From e01e6e6c5e706ddbacf7e7ecde7408d939fb8588 Mon Sep 17 00:00:00 2001 From: isvogor Date: Mon, 17 Apr 2017 11:51:07 -0400 Subject: [PATCH 01/26] initialization --- src/buzzuav_closures.cpp | 1 + src/roscontroller.cpp | 16 +++++++++++----- src/testflockfev.bzz | 2 +- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 6fa15b9..2f1f754 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -109,6 +109,7 @@ namespace buzzuav_closures{ printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); gps_from_rb(d, b, goto_pos); cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/ + printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]); buzz_cmd= COMMAND_MOVETO; // TO DO what should we use return buzzvm_ret0(vm); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 47473d1..1e086b3 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -26,6 +26,9 @@ namespace rosbzz_node{ GetRobotId(); setpoint_counter = 0; fcu_timeout = TIMEOUT; + home[0] = 0.0; + home[1] = 0.0; + home[2] = 0.0; } /*--------------------- @@ -43,6 +46,7 @@ namespace rosbzz_node{ void roscontroller::GetRobotId() { + /* mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id"; mavros_msgs::ParamGet::Response robot_id_srv_response; while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){ @@ -51,8 +55,8 @@ namespace rosbzz_node{ } robot_id=robot_id_srv_response.value.integer; - - //robot_id = 8; + */ + robot_id = 8; } /*------------------------------------------------- @@ -512,9 +516,9 @@ namespace rosbzz_node{ void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){ double d_lon = nei[1] - cur[1]; - double d_lat = nei[0] - cur[0]; - out[0] = DEG2RAD(d_lat) * EARTH_RADIUS; - out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); + double d_lat = nei[0] - cur[0]; + out[0] = DEG2RAD(d_lat) * EARTH_RADIUS; + out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); out[2] = cur[2]; } @@ -796,6 +800,8 @@ namespace rosbzz_node{ moveMsg.pose.orientation.z = 0; moveMsg.pose.orientation.w = 1; + std::cout<<"Home (0,1,2): " << home[0] <<", " << home[1] << ", " << home[2] < Date: Thu, 27 Apr 2017 12:36:12 -0400 Subject: [PATCH 02/26] set home only once --- launch/rosbuzz-testing-sitl.launch | 2 ++ src/roscontroller.cpp | 23 ++++++++++++++++------- src/testflockfev.bzz | 10 +++++++--- 3 files changed, 25 insertions(+), 10 deletions(-) diff --git a/launch/rosbuzz-testing-sitl.launch b/launch/rosbuzz-testing-sitl.launch index 141f0a0..e086595 100644 --- a/launch/rosbuzz-testing-sitl.launch +++ b/launch/rosbuzz-testing-sitl.launch @@ -33,6 +33,8 @@ + + diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index abd99e4..a948de0 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -74,6 +74,7 @@ namespace rosbzz_node{ ////////////////////////////////////////////////////// while (ros::ok() && !buzz_utility::buzz_script_done()) { + std::cout<(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); @@ -425,12 +426,19 @@ namespace rosbzz_node{ break; case mavros_msgs::CommandCode::NAV_TAKEOFF: if(!armstate){ + SetMode("LOITER", 0); armstate = 1; Arm(); ros::Duration(0.5).sleep(); - // Registering HOME POINT. - home[0] = cur_pos[0];home[1] = cur_pos[1];home[2] = cur_pos[2]; + // Registering HOME POINT. + if(home[0] == 0){ + //test #1: set home only once -- ok + home[0] = cur_pos[0]; home[1] = cur_pos[1]; home[2] = cur_pos[2]; + //test #2: set home mavros -- nope + //SetMavHomePosition(cur_pos[0], cur_pos[1], cur_pos[2]); + + } } if(current_mode != "GUIDED") SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm) @@ -510,9 +518,9 @@ namespace rosbzz_node{ void roscontroller::set_cur_pos(double latitude, double longitude, double altitude){ - cur_pos [0] =latitude; - cur_pos [1] =longitude; - cur_pos [2] =altitude; + cur_pos [0] = latitude; + cur_pos [1] = longitude; + cur_pos [2] = altitude; } /*----------------------------------------------------------- @@ -781,6 +789,7 @@ namespace rosbzz_node{ } } + void roscontroller::SetStreamRate(int id, int rate, int on_off){ mavros_msgs::StreamRate message; message.request.stream_id = id; diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz index f3b58f8..c3573cd 100644 --- a/src/testflockfev.bzz +++ b/src/testflockfev.bzz @@ -49,8 +49,12 @@ function hexagon() { timeW =0 statef=land } else { - timeW = timeW+1 - uav_moveto(0.2,0.0) + if(timeW >= (WAIT_TIMEOUT / 2)){ + uav_moveto(5.0,0.0) + } else { + uav_moveto(0.0,5.0) + } + timeW = timeW+1 } } @@ -86,7 +90,7 @@ function barrier_ready() { # # Executes the barrier # -WAIT_TIMEOUT = 200 +WAIT_TIMEOUT = 300 timeW=0 function barrier_wait(threshold, transf) { barrier.get(id) From 817b2951d6821c58258e82db630809a227b617fb Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Wed, 3 May 2017 15:07:36 -0400 Subject: [PATCH 03/26] fix test bzz --- src/roscontroller.cpp | 3 ++- src/testflockfev.bzz | 16 ++++++++-------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index d4f0830..26f5508 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -150,7 +150,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? diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz index 30e5b80..1845eb9 100644 --- a/src/testflockfev.bzz +++ b/src/testflockfev.bzz @@ -43,15 +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 { - timeW = timeW+1 - uav_moveto(0.0,0.0) - } +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } } ######################################## From 50327cb6b680c230ab6c5dae2c6f1e150286bb15 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 6 May 2017 18:42:27 -0400 Subject: [PATCH 04/26] Swarm table fix --- launch/rosbuzzm100.launch | 10 ++++++---- src/buzz_utility.cpp | 12 +++++++----- src/roscontroller.cpp | 12 ++++++------ 3 files changed, 19 insertions(+), 15 deletions(-) diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch index c44b99e..0c0b0e7 100644 --- a/launch/rosbuzzm100.launch +++ b/launch/rosbuzzm100.launch @@ -2,15 +2,17 @@ - + + - + - - + + + diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index c9cbd69..cf39566 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -450,8 +450,8 @@ buzzvm_dup(VM); 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, "CHECKING SWARM :%i, memeber: %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; } @@ -504,16 +504,18 @@ buzzvm_dup(VM); buzz_error_info()); buzzvm_dump(VM); } + /* Update swarm membership */ + buzzswarm_members_update(VM->swarmmembers); //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); + buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); //int SwarmSize = buzzdict_size(VM->swarmmembers)+1; //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize); /* 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/roscontroller.cpp b/src/roscontroller.cpp index 26f5508..faf8c29 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -26,7 +26,7 @@ namespace rosbzz_node{ 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; @@ -268,16 +268,16 @@ namespace rosbzz_node{ bzzfile_in_compile.str(""); std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); name = name.substr(0,name.find_last_of(".")); - bzzfile_in_compile << "bzzparse "< Date: Sat, 6 May 2017 23:07:52 -0400 Subject: [PATCH 05/26] updates addition and swarm table correction --- include/buzz_utility.h | 6 +- launch/rosbuzzm100.launch | 2 +- src/buzz_update.cpp | 361 ++++++++++++++++++-------------------- src/buzz_utility.cpp | 320 ++++++++++++++++----------------- src/roscontroller.cpp | 20 +-- 5 files changed, 333 insertions(+), 376 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 3667363..60df181 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -32,9 +32,11 @@ void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map); int buzz_update_users_stigmergy(buzzobj_t tbl); -void in_msg_process(uint64_t* payload); +void in_msg_append(uint64_t* payload); -uint64_t* out_msg_process(); +uint64_t* obt_out_msg(); + +void update_sensors(); int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id); diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch index 0c0b0e7..01d9402 100644 --- a/launch/rosbuzzm100.launch +++ b/launch/rosbuzzm100.launch @@ -13,7 +13,7 @@ - + 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 cf39566..a6297cf 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -45,10 +45,10 @@ namespace buzz_utility{ 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; + 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 +150,8 @@ namespace buzz_utility{ cout<<" payload from out msg "<<*(payload_64+i)<pc, VM->errormsg); } - return msg; + return msg; } /****************************************/ @@ -254,87 +253,86 @@ namespace buzz_utility{ /****************************************/ /*Sets the .bzz and .bdbg file*/ /****************************************/ - int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id) { - cout << " Robot ID: " <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; - } - } - } + buzzswarm_elem_t e = *(buzzswarm_elem_t*)data; + int* status = (int*)params; + if(*status == 3) return; + fprintf(stderr, "CHECKING SWARM :%i, memeber: %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); + buzzobj_t tbl = buzzuav_closures::buzzuav_update_userspos(VM); + buzzuav_closures::buzzuav_update_flight_status(VM); + //buzz_update_users_stigmergy(tbl); + } 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); - } - /* Update swarm membership */ - buzzswarm_members_update(VM->swarmmembers); - //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); - /* 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); } /****************************************/ @@ -565,26 +560,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/roscontroller.cpp b/src/roscontroller.cpp index faf8c29..5b7bb9c 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -22,11 +22,11 @@ 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; @@ -67,7 +67,7 @@ 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()); + init_update_monitor(bcfname.c_str(),stand_by.c_str()); /////////////////////////////////////////////////////// // MAIN LOOP ////////////////////////////////////////////////////// @@ -78,7 +78,7 @@ namespace rosbzz_node{ /*Neighbours of the robot published with id in respective topic*/ neighbours_pos_publisher(); /*Check updater state and step code*/ - //update_routine(bcfname.c_str(), dbgfname.c_str()); + update_routine(bcfname.c_str(), dbgfname.c_str()); /*Step buzz script */ buzz_utility::buzz_script_step(); /*Prepare messages and publish them in respective topic*/ @@ -86,10 +86,10 @@ namespace rosbzz_node{ /*call flight controler service to set command long*/ flight_controller_service_call(); /*Set multi message available after update*/ - /*if(get_update_status()){ + if(get_update_status()){ set_read_update_status(); multi_msg=true; - }*/ + } /*Set ROBOTS variable for barrier in .bzz from neighbours count*/ //no_of_robots=get_number_of_robots(); get_number_of_robots(); @@ -334,7 +334,7 @@ namespace rosbzz_node{ /*----------------------------------------------------------------------------------------------------*/ void roscontroller::prepare_msg_and_publish(){ /*obtain Pay load to be sent*/ - uint64_t* payload_out_ptr= buzz_utility::out_msg_process(); + uint64_t* payload_out_ptr= buzz_utility::obt_out_msg(); uint64_t position[3]; /*Appened current position to message*/ memcpy(position, cur_pos, 3*sizeof(uint64_t)); @@ -358,7 +358,7 @@ namespace rosbzz_node{ delete[] out; delete[] payload_out_ptr; /*Check for updater message if present send*/ - /*if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){ + if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){ uint8_t* buff_send = 0; uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());; int tot=0; @@ -394,7 +394,7 @@ namespace rosbzz_node{ payload_pub.publish(update_packets); multi_msg=false; delete[] payload_64; - }*/ + } } /*--------------------------------------------------------------------------------- @@ -855,7 +855,7 @@ namespace rosbzz_node{ raw_neighbours_pos_put((int)out[1],raw_neigh_pos); neighbours_pos_put((int)out[1],n_pos); delete[] out; - buzz_utility::in_msg_process((message_obt+3)); + buzz_utility::in_msg_append((message_obt+3)); } } From 5070268491ca65cf4cfdaf3b03c9c313679e26a2 Mon Sep 17 00:00:00 2001 From: dave Date: Sat, 6 May 2017 23:40:50 -0400 Subject: [PATCH 06/26] vstig tables work --- include/buzz_utility.h | 12 ++ src/buzz_utility.cpp | 292 ++++++++++++++++++++++++++++++--------- src/buzzuav_closures.cpp | 16 ++- src/roscontroller.cpp | 37 ++--- src/testflocksim.bzz | 14 +- 5 files changed, 284 insertions(+), 87 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 3667363..1020a67 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); @@ -29,6 +35,12 @@ int buzz_listen(const char* type, int msg_size); void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map); +void update_neighbors(); +void add_user(int id, double latitude, double longitude, float altitude); +void update_users(); +int make_table(buzzobj_t* t); +int buzzusers_add(int id, double latitude, double longitude, double altitude); +int buzzusers_reset(); int buzz_update_users_stigmergy(buzzobj_t tbl); diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index c9cbd69..242690d 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,146 @@ 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; + std::map< int, Pos_struct> neighbors_map; /****************************************/ /*adds neighbours position*/ void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map){ + neighbors_map.clear(); + map< int, Pos_struct >::iterator it; + for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){ + neighbors_map.insert(make_pair(it->first,it->second)); + } + } + + /* update at each step the VM table */ + void update_neighbors(){ /* Reset neighbor information */ - buzzneighbors_reset(VM); + 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); - } + for (it=neighbors_map.begin(); it!=neighbors_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); + ROS_INFO("Buzz_utility got new user: %i", id); + users_map.insert(make_pair(id, pos_arr)); + } + + void update_users(){ + /* Reset neighbor information */ + buzzusers_reset(); + /* Get robot id and update neighbor information */ + map< int, Pos_struct >::iterator it; + for (it=users_map.begin(); it!=users_map.end(); ++it){ + buzzusers_add(it->first, + (it->second).x, + (it->second).y, + (it->second).z); + } + } + + int buzzusers_reset() { + if(VM->state != BUZZVM_STATE_READY) return VM->state; + /* Make new table */ + buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE); + //make_table(&t); + if(VM->state != BUZZVM_STATE_READY) return VM->state; + /* Register table as global symbol */ + buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); + buzzvm_gload(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); + buzzvm_tget(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1)); + //buzzvm_gload(VM); + buzzvm_push(VM, t); + buzzvm_pushi(VM, 2); + buzzvm_call(VM, 0); + //buzzvm_gstore(VM); + return VM->state; + } + + int buzzusers_add(int id, double latitude, double longitude, double altitude) { + if(VM->state != BUZZVM_STATE_READY) return VM->state; + /* Get users "p" table */ + buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); + buzzvm_gload(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1)); + buzzvm_tget(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1)); + buzzvm_gload(VM); + buzzvm_pushi(VM, 1); + buzzvm_call(VM, 0); + buzzvm_type_assert(VM, 1, BUZZTYPE_TABLE); + buzzobj_t nbr = buzzvm_stack_at(VM, 1); + /* Get "data" field */ + buzzvm_pushs(VM, buzzvm_string_register(VM, "data", 1)); + buzzvm_tget(VM); + if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) { + /* Empty data, create a new table */ + buzzvm_pop(VM); + buzzvm_push(VM, nbr); + buzzvm_pushs(VM, buzzvm_string_register(VM, "data", 1)); + buzzvm_pusht(VM); + buzzobj_t data = buzzvm_stack_at(VM, 1); + buzzvm_tput(VM); + buzzvm_push(VM, data); + } + /* When we get here, the "data" table is on top of the stack */ + /* Push user id */ + buzzvm_pushi(VM, id); + /* Create entry table */ + buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE); + /* Insert latitude */ + buzzvm_push(VM, entry); + buzzvm_pushs(VM, buzzvm_string_register(VM, "la", 1)); + buzzvm_pushf(VM, latitude); + buzzvm_tput(VM); + /* Insert longitude */ + buzzvm_push(VM, entry); + buzzvm_pushs(VM, buzzvm_string_register(VM, "lo", 1)); + buzzvm_pushf(VM, longitude); + buzzvm_tput(VM); + /* Insert altitude */ + buzzvm_push(VM, entry); + buzzvm_pushs(VM, buzzvm_string_register(VM, "al", 1)); + buzzvm_pushf(VM, altitude); + buzzvm_tput(VM); + /* Save entry into data table */ + buzzvm_push(VM, entry); + buzzvm_tput(VM); + return VM->state; } /**************************************************************************/ /*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */ /**************************************************************************/ 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 " <state; } /**************************************************/ @@ -224,15 +333,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,17 +356,59 @@ namespace buzz_utility{ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - return BUZZVM_STATE_READY; + + return VM->state; } +static int create_stig_tables() { + // usersvstig = stigmergy.create(123) + buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); + //buzzvm_gstore(VM); + // get the stigmergy table from the global scope + buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1)); + buzzvm_gload(VM); + // get the create method from the stigmergy table + buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1)); + buzzvm_tget(VM); + // value of the stigmergy id + buzzvm_pushi(VM, 5); + // call the stigmergy.create() method +// buzzvm_closure_call(VM, 1); + buzzvm_pushi(VM, 1); + buzzvm_call(VM, 0); + buzzvm_gstore(VM); + + /*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); + buzzvm_gload(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); + buzzvm_tget(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1)); + buzzvm_pusht(VM); + buzzvm_pushi(VM, 2); + buzzvm_call(VM, 0); + //buzzvm_gstore(VM);*/ + buzzusers_reset(); + + /*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); + buzzvm_gload(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); + buzzvm_tget(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "u", 1)); + buzzvm_pusht(VM); + buzzvm_pushi(VM, 2); + buzzvm_call(VM, 0); + buzzvm_gstore(VM);*/ + + return VM->state; +} /****************************************/ /*Sets the .bzz and .bdbg file*/ /****************************************/ int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id) { - cout << " Robot ID: " <errormsg << endl; + //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; + return 0; + } /* Save bytecode file name */ BO_FNAME = strdup(bo_filename); @@ -336,24 +473,47 @@ buzzvm_dup(VM); buzzvm_function_call(VM, "init", 0); /* All OK */ + ROS_INFO("[%i] INIT DONE!!!", Robot_id); + return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size); } - int buzz_update_users_stigmergy(buzzobj_t tbl){ - // push the key (here it's an int with value 46) + /****************************************/ + /*Update users positions */ + /****************************************/ + + 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); + buzzvm_push(VM, peoplevstig); // get the put method from myvstig buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); buzzvm_tget(VM); // call the vstig.put() method - buzzvm_closure_call(VM, 2); + buzzvm_closure_call(VM, 2);*/ + + + // put something in it.... + //buzzvm_push(VM, peoplevstig); + buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1)); + buzzvm_gload(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); + buzzvm_tget(VM); + //buzzvm_pushs(VM, buzzvm_string_register(VM, "id", 1)); + buzzvm_pushi(VM, Robot_id); + //buzzvm_gload(VM); + buzzvm_push(VM, tbl); + buzzvm_pushi(VM, 2); + buzzvm_call(VM, 0); + //buzzvm_closure_call(VM, 2); + //buzzvm_gstore(VM); return 1; } + /****************************************/ /*Sets a new update */ /****************************************/ @@ -489,10 +649,12 @@ buzzvm_dup(VM); buzzuav_closures::buzzuav_update_battery(VM); buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_currentpos(VM); - buzzobj_t tbl = buzzuav_closures::buzzuav_update_userspos(VM); + update_neighbors(); + update_users(); + buzzuav_closures::buzzuav_update_userspos(VM); buzzuav_closures::buzzuav_update_flight_status(VM); - - //buzz_update_users_stigmergy(tbl); + + //buzz_update_users_stigmergy(tbl); /* diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 5d1433f..8f6b751 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 @@ -246,6 +246,14 @@ namespace buzzuav_closures{ cur_pos[2]=altitude; } void set_userspos(double latitude, double longitude, double altitude){ + /*buzz_utility::Pos_struct pos_arr; + pos_arr.x=latitude; + pos_arr.y=longitude; + pos_arr.z=altitude; + map< int, buzz_utility::Pos_struct >::iterator it = users_map.find(id); + if(it!=users_map.end()) + users_map.erase(it); + users_map.insert(make_pair(id, pos_arr));*/ users_pos[0]=latitude; users_pos[1]=longitude; users_pos[2]=altitude; @@ -270,6 +278,9 @@ namespace buzzuav_closures{ return vm->state; } buzzobj_t buzzuav_update_userspos(buzzvm_t vm) { + /*map< int, buzz_utility::Pos_struct >::iterator it; + for (it=users_map.begin(); it!=users_map.end(); ++it){ + }*/ buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1)); buzzvm_pusht(vm); buzzvm_dup(vm); @@ -284,9 +295,10 @@ namespace buzzuav_closures{ buzzvm_pushs(vm, buzzvm_string_register(vm, "height", 1)); buzzvm_pushf(vm, users_pos[2]); buzzvm_tput(vm); + buzzobj_t tbl = buzzvm_stack_at(vm, 0); buzzvm_gstore(vm); - return buzzvm_stack_at(vm, 0); + return tbl; //return vm->state; } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index d4f0830..96dfaf2 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -29,7 +29,8 @@ namespace rosbzz_node{ robot_id=strtol(robot_name.c_str() + 5, NULL, 10);; setpoint_counter = 0; fcu_timeout = TIMEOUT; - home[0]=0.0;home[1]=0.0;home[2]=0.0; + + home[0]=0.0;home[1]=0.0;home[2]=0.0; } /*--------------------- @@ -48,7 +49,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"); @@ -73,14 +74,14 @@ namespace rosbzz_node{ ////////////////////////////////////////////////////// while (ros::ok() && !buzz_utility::buzz_script_done()) { - /*Update neighbors position inside Buzz*/ - buzz_utility::neighbour_pos_callback(neighbours_pos_map); + /*Update neighbors position inside Buzz*/ + buzz_utility::neighbour_pos_callback(neighbours_pos_map); /*Neighbours of the robot published with id in respective topic*/ neighbours_pos_publisher(); /*Check updater state and step code*/ //update_routine(bcfname.c_str(), dbgfname.c_str()); /*Step buzz script */ - buzz_utility::buzz_script_step(); + buzz_utility::buzz_script_step(); /*Prepare messages and publish them in respective topic*/ prepare_msg_and_publish(); /*call flight controler service to set command long*/ @@ -97,8 +98,8 @@ namespace rosbzz_node{ 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(); @@ -107,10 +108,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); @@ -218,8 +218,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); @@ -261,7 +261,7 @@ namespace rosbzz_node{ /*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("\\/")); + 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)); } /*----------------------------------------------------------------------------------- @@ -499,7 +499,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)); } @@ -709,7 +709,8 @@ namespace rosbzz_node{ us[2] = data.pos_neigh[it].altitude; double out[3]; cvt_rangebearing_coordinates(us, out, cur_pos); - buzzuav_closures::set_userspos(out[0], out[1], out[2]); + //buzzuav_closures::set_userspos(out[0], out[1], out[2]); + buzz_utility::add_user(data.pos_neigh[it].position_covariance_type,data.pos_neigh[it].latitude, data.pos_neigh[it].longitude, data.pos_neigh[it].altitude); } } @@ -757,7 +758,7 @@ namespace rosbzz_node{ moveMsg.pose.orientation.w = 1; // To prevent drifting from stable position. - if(fabs(x)>0.1 || fabs(y)>0.1) { + if(fabs(x)>0.01 || fabs(y)>0.01) { localsetpoint_nonraw_pub.publish(moveMsg); ROS_INFO("Sent local NON RAW position message!"); } diff --git a/src/testflocksim.bzz b/src/testflocksim.bzz index 107edbf..05fe7c5 100644 --- a/src/testflocksim.bzz +++ b/src/testflocksim.bzz @@ -141,6 +141,12 @@ function land() { } } +function table_print(t) { + foreach(t, function(key, value) { + log(key, " -> ", value) + }) +} + # Executed once at init time. function init() { s = swarm.create(1) @@ -199,9 +205,13 @@ neighbors.listen("cmd", log("User position: ", users.range) # Read a value from the structure - #x = usersvstig.get(46) + t = vt.get("p") + log(t) + #table_print(t) + #t = vt.get("u") + #table_print(t) # Get the number of keys in the structure - #log("The vstig has ", usersvstig.size(), " elements") + log("The vstig has ", vt.size(), " elements") } # Executed once when the robot (or the simulator) is reset. From 75af18d17e8ac5e6adcecab46e6e4f0824e1ee62 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sun, 7 May 2017 15:08:57 -0400 Subject: [PATCH 07/26] addition of v-stig table into update parts --- src/buzz_utility.cpp | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 63b82fd..33645a6 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -488,7 +488,15 @@ static int create_stig_tables() { fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); return 0; } - + //v-stig table + 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 @@ -529,7 +537,15 @@ static int create_stig_tables() { fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); return 0; } - + //v-stig table + 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 From 2566ac5de2f16bddf4777ac683d01cc4559ef113 Mon Sep 17 00:00:00 2001 From: dave Date: Sun, 7 May 2017 16:02:23 -0400 Subject: [PATCH 08/26] more work on vstig --- src/buzz_utility.cpp | 90 +++++++++++++++++++++++++++++++------------- src/testflocksim.bzz | 7 ++-- 2 files changed, 67 insertions(+), 30 deletions(-) diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 63b82fd..8cd37f6 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -39,17 +39,24 @@ namespace buzz_utility{ } void update_users(){ - /* Reset neighbor information */ - buzzusers_reset(); - /* Get robot id and update neighbor information */ - map< int, Pos_struct >::iterator it; - for (it=users_map.begin(); it!=users_map.end(); ++it){ - //ROS_INFO("Buzz_utility will save user %i.", it->first); - buzzusers_add(it->first, - (it->second).x, - (it->second).y, - (it->second).z); - } + 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() { @@ -67,8 +74,7 @@ namespace buzz_utility{ //buzzvm_gload(VM); buzzvm_push(VM, t); buzzvm_pushi(VM, 2); - buzzvm_call(VM, 0); - //buzzvm_gstore(VM); + buzzvm_callc(VM); return VM->state; } @@ -82,15 +88,14 @@ namespace buzz_utility{ buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1)); //buzzvm_gload(VM); buzzvm_pushi(VM, 1); - buzzvm_call(VM, 0); + buzzvm_callc(VM); buzzvm_type_assert(VM, 1, BUZZTYPE_TABLE); buzzobj_t nbr = buzzvm_stack_at(VM, 1); - //buzzvm_dump(VM); /* Get "data" field */ buzzvm_pushs(VM, buzzvm_string_register(VM, "data", 1)); buzzvm_tget(VM); if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) { - /* Empty data, create a new table */ + ROS_INFO("Empty data, create a new table"); buzzvm_pop(VM); buzzvm_push(VM, nbr); buzzvm_pushs(VM, buzzvm_string_register(VM, "data", 1)); @@ -123,7 +128,17 @@ namespace buzz_utility{ buzzvm_push(VM, entry); buzzvm_tput(VM); ROS_INFO("Buzz_utility saved new user: %i (%f,%f,%f)", id, latitude, longitude, altitude); - //buzzvm_dump(VM); + // 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; } /**************************************************************************/ @@ -355,19 +370,22 @@ static int create_stig_tables() { // call the stigmergy.create() method // buzzvm_closure_call(VM, 1); buzzvm_pushi(VM, 1); - buzzvm_call(VM, 0); + buzzvm_callc(VM); buzzvm_gstore(VM); - /*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); - buzzvm_gload(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); + //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_pusht(VM); + buzzvm_push(VM, t); +// buzzvm_closure_call(VM, 2); buzzvm_pushi(VM, 2); - buzzvm_call(VM, 0); - //buzzvm_gstore(VM);*/ - buzzusers_reset(); + buzzvm_callc(VM); + //buzzvm_gstore(VM); + //buzzvm_dump(VM); /*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); buzzvm_gload(VM); @@ -433,7 +451,7 @@ static int create_stig_tables() { ROS_ERROR("[%i] Error registering hooks", Robot_id); return 0; } - + /* Create vstig tables */ if(create_stig_tables() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); @@ -488,6 +506,15 @@ static int create_stig_tables() { 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); @@ -529,6 +556,15 @@ static int create_stig_tables() { 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); @@ -657,6 +693,8 @@ static int create_stig_tables() { 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); diff --git a/src/testflocksim.bzz b/src/testflocksim.bzz index 05fe7c5..fa5f3f5 100644 --- a/src/testflocksim.bzz +++ b/src/testflocksim.bzz @@ -206,10 +206,9 @@ neighbors.listen("cmd", # Read a value from the structure t = vt.get("p") - log(t) - #table_print(t) - #t = vt.get("u") - #table_print(t) + log(t) + table_print(t) + # Get the number of keys in the structure log("The vstig has ", vt.size(), " elements") } From f8917cd39eb188a2c8e209ea000086b543cdb08b Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 9 May 2017 13:07:11 -0400 Subject: [PATCH 09/26] fix bzz compilation and moved scripts and include --- .gitignore | 4 + include/buzzuav_closures.h | 2 - include/roscontroller.h | 2 +- script/include/string.bzz | 92 +++++++++++++++ script/include/vec2.bzz | 107 +++++++++++++++++ script/stand_by.bdb | Bin 0 -> 7810 bytes script/stand_by.bo | Bin 0 -> 572 bytes {src => script}/stand_by.bzz | 0 {src => script}/test.bzz | 0 {src => script}/test1.bzz | 0 {src => script}/testalone.bzz | 0 {src => script}/testflockfev.bzz | 0 {src => script}/testflockfev_barrier.bzz | 0 {src => script}/testflocksim.bzz | 30 +++-- script/teststig.bdb | Bin 0 -> 9585 bytes script/teststig.bo | Bin 0 -> 678 bytes script/teststig.bzz | 38 ++++++ src/buzz_utility.cpp | 52 ++++---- src/buzzuav_closures.cpp | 39 +----- src/roscontroller.cpp | 51 ++++---- src/stand_by.basm | 144 ----------------------- src/stand_by.bdb | Bin 7560 -> 0 bytes src/stand_by.bdbg | Bin 6840 -> 0 bytes src/stand_by.bo | Bin 531 -> 0 bytes 24 files changed, 326 insertions(+), 235 deletions(-) create mode 100644 script/include/string.bzz create mode 100644 script/include/vec2.bzz create mode 100644 script/stand_by.bdb create mode 100644 script/stand_by.bo rename {src => script}/stand_by.bzz (100%) rename {src => script}/test.bzz (100%) rename {src => script}/test1.bzz (100%) rename {src => script}/testalone.bzz (100%) rename {src => script}/testflockfev.bzz (100%) rename {src => script}/testflockfev_barrier.bzz (100%) rename {src => script}/testflocksim.bzz (90%) create mode 100644 script/teststig.bdb create mode 100644 script/teststig.bo create mode 100644 script/teststig.bzz delete mode 100644 src/stand_by.basm delete mode 100644 src/stand_by.bdb delete mode 100644 src/stand_by.bdbg delete mode 100644 src/stand_by.bo 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 0000000000000000000000000000000000000000..de2da98de2aaa76c5668d25c4392924ef77479a1 GIT binary patch literal 7810 zcmb`MJ8X?{7{*VFUX)&Fi=Mhv(st1HYm?HXh@=Y^5=SByM=vLF37Vd^IEjVDf`x^J zg~h_cf`x^Jfx#t&NJJPIbg9JS_}+Nt=Y9W6zvX@UJiqsU`@S<*k;$}V{*5I4JCI2Z zoj6uVCPrrq$)4T0{JvaLoErc8I5JZzCB^Zn!;{lVaeDOlM1Jga-&m>C51Fd8e5o#T z88VG&d9^Mx02wQ{ze1=hj`m}j(CnjIc^k>5X$C@;|xDQ4R@TH`52jQLfoq(o+reP zZR;R~a>zW6>C|ru#% zD`78wMs8N>p%S<;CXS2A5o&mc?xq?t#>89+Y>twfPTfsSjGO0>u`S)vslk{|)xyo9 zl!@c}%?rrbF}FZRypcjVwyjCHvDw?LyQz<1^9nN7XgrpOFpGy{*fdZmV#2| zcaX7*tmCV|8OYf58`49y#f1786Dmt?9N&vSksHU!a27HFh|fXBUVv;-2-9yJv9T|; zg3K3UW0yBU<~)UR{C3wF6Y4vKvh7O<)dd+_l8$ZbLJXUWkg=Nt9@TB@Cu(iKS)+f>|e( zIFQ45c4lY2&IK*YEO9Dp&PkHKuBj7j2ilvTPyoG}UK*(+2Wvqa) z^z|cehP*90?*Kz@#FFpxF0h``^zBLC-~rh>9}HzH-WPc&nVR bvAnupJ7S03(G5=jb`C{|0{_Fq`tslxS&C1c literal 0 HcmV?d00001 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 0000000000000000000000000000000000000000..cc42d708f7a9fee87df7cfb0952c29a7bd454329 GIT binary patch literal 9585 zcmb_gJ#39(6#l4It5w>n(jTcyZb|6pr%Oo?Q4%4gH&KHUy}i*ZsW!b|CGMbIT7w$Y zz@P@LfrZ6l!NOu;VPUbbu&@|#T<=Mq`8?-a?p@v|&(C?!d){~3k|?T-{(tH4yEBT1 zM@Mq;(D}=`xUV;Ju0InO#h%n4chOvmItmU{;6aRd$$L0JFjlGvH(KE+313fyF!QqRxUSEki>U z?-n+iko+xu2AD2cTN9R7*kSGnjMmL6JIo(}QBMeqvpyC_0@D=4EVqjq5}1~-ToW@S z)eT9lw6mG_N$MWJXz&J6t?|x@I`^C3oOd* zY^r@M9tDeuJ#`2yUSk(k1EPq)$3PSjcn^po=9`B;fjrqefjh-a>2`nZ8KjFjXq`+wP3bE2>sGFyd6j3*m zK1sFuSUe>#ZNZaec5m8%O@kfgluuI6ASq(n>IXJuc2Uzl7N>kHo&h$*$?)9A;!VP4 zU9gtuerF&l;_C52V6dOi_5{57yqj|>*ygusa%-U)&0S#Dr62W+bCJG@N1QBU-V z)xahOQN%OQFa)lbMFX2R5V*e78endN#e}Fzu$UNVyM)d95H!&!s)UX1C8DK|_$2k# zC#g%oMvpy1;O~HqKHM2#?)ubCHv~@1e)GVFfTl%aGK2pTPHm zD57m8AWC<-LDUC<(YJJB6u9qGHx0r@>xMX|7KF`)(ES9l^clQ407((IyG4OX@oXAJ zi5|K{FTNr$Iwlcq>!ZLp)Xf9&Ce3^E3D~GdhQNCuaJ_;UV48iBy6ThEVGyN>8AP>! zD59lL`iS~0Fe-+q;v?dX&V0n4ni4iT^XYtSu(%y8Ci=}6A8(HNc(V~i5lKyh#l#N( z3f{DazGF!0ICw*Bh8Yk=l;1Uh(P51!zqG)pwM0_iz~Xkh#eHBgar5l}QF_oA0{;%8 aQg%@p5T%E(fz7P2aR~ed!06OvVDlFoI_WL| literal 0 HcmV?d00001 diff --git a/script/teststig.bo b/script/teststig.bo new file mode 100644 index 0000000000000000000000000000000000000000..63e7db39574f1a5b89cc74026439d9ba2fbefdaa GIT binary patch literal 678 zcmZ`#%TmHX5NyQwexc$963uw>2RwQ72dEP6R3KX+N)jp?R6O}H`XTc@E?8(~1`5syHlkmQG-CR8|I~s*ICyY-(ghR>Q!r-QrFj+} z%dEgNOr527q$&2vP*;sm^s# z8>E`k64dz&uc)-)YobkEl?zZ4z;(L;__QVT*3y>HFH1wA9~)x06#6L?#1zB^mRUd~ zcfUcKd?@l517ctA5Yj#)9WW?zb{V{~A2RRw z)}!c%F997hlIY98KgAQ)UEXq;Q!&z+^ql|dadKN^$yap27}y7fq?kwY&q8uOXYs{C vKMA4Rkvc3Z%Ud_^oJw65I!y}ruv5Bg_7~5;`^)O?XRgVY=euNwp67i78Yx+$ literal 0 HcmV?d00001 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<_{u+Vv z8=Q`8ey70cGrradoU6vyT7mNdbAd|7%IxXMdEVsHEpYn5Y0Bm=7dY2~@_7Rd%xArz z=Q<#*+5BdK^A>ZN%PN7h06nbdK7lg;PQ-fc1c$lcH65Nhyn)Hgxn1CVz+C1`e;z%% zpojY^FX(B2n27bt2XSr&=Bse_n0%C+Z<~BJ3pp2q^4V*ADd&Vc#@80XSF`b@^q;S0-IepkK9dj6Q=U4! z8rGWjU|aL)MR_t3L19vNRsAA4*#8=-A+c=Q}L`IMG0 zF3um!<$hQ4Ib!s533~np>3I^A^E~tvGC#am@T}KU=wUx?5b`-{a_$j03qih~Ll5g1 zuj%mQ^UvsEzxQw|CZAqG&+#D63BytTmtTStk#im#*0XXBA2GfXA?H`%@cW0?ba>Xi zV03jf2BF9uRyr1^Idx6tLxdsl;soWo)I&3ld z@IJ}I`DpUtJ%@*L+W1;6)M0y&uP??I`Or!t+ diff --git a/src/stand_by.bdbg b/src/stand_by.bdbg deleted file mode 100644 index cda97bcb64b2a515a24a8d21320eed0422c95f7a..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6840 zcmb`LJ8X?{7{e7}diINCD3=Aw5m&L+jarD#?&7qBRs^ugW78WclEG(8* zA`)?lNJQc;!JsTGEG!llgU8qZlf3i$y#J*6{>%Hz^F06Wy#Ke~^DN7z;_v@jF!{(f zW%-ezW0iblV5Cyb`}>Bg1B0c}dS0*f=e41F`9z~p?CZmQgNI7xQ|rr(#-*k#>qzx8 z1Wpc4YpP!;a5e;S-arHM=@#^CGf&2Tye&Q_Dp3V}0$TE5RpKHH3*UO~^NAU)TEa{dfG(~_SS!PgDwVLzQBaJHMA zR|}jksEzP-6M7aW9oQDjb6}Q^w+e|+5g+6vXD4%wthu^n6zG_B~GDm#`hv%i_JOR$~lrOf$^7!gN zF?BQxoKbN2`%>~b2@dBlh8=XPyq`UsyCxr{k99*2 z>y`WBOpvdK#uvZec=G8nzBs>lIA@J7-m4zYBlu#z%-h3RX?!X7;5p+<`5xYD^eFe> hd80@9{`|z`tlSrCj4$>@&v{)4^7Smp*To<`{{VNFf)xM& diff --git a/src/stand_by.bo b/src/stand_by.bo deleted file mode 100644 index 62e6638cfa6596f03a85929fa44263ac96d0a366..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 531 zcmZut%TB{E5ZnR|Z%TmTNL0$AM)VuF@&lECdz6P8EK(LqV%2es~;GSFIW93WPW!=-IjMB#Dquy}r=v}yr& z0pRwBAi@)-Un8ayroj$TK%#)8l1u``e2+%BM+XW5i2>nFtO#SCn?~N0xX(Hs5K3>v zoS)($VZUXVJL0+7J!Sj6NVpXqvz&0Ir<4EXa6Esfs2>dOKACbn5X1IUUh}6#UtloL z*z{$n^_VUa&;2|B$k{h{@~s7i-n}49NCmtki20>y-;u{;I2UyKPsA%0BH{n5?B5W- Dfn`a` From 149de3d60b3225bce8bc434e86dcadc10130b804 Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 9 May 2017 13:28:32 -0400 Subject: [PATCH 10/26] fix bzz include --- script/testflocksim.bzz | 2 +- script/teststig.bdb | Bin 9585 -> 27349 bytes script/teststig.bo | Bin 678 -> 1438 bytes script/teststig.bzz | 7 ++++--- src/roscontroller.cpp | 2 +- 5 files changed, 6 insertions(+), 5 deletions(-) diff --git a/script/testflocksim.bzz b/script/testflocksim.bzz index 2dcca47..616de9c 100644 --- a/script/testflocksim.bzz +++ b/script/testflocksim.bzz @@ -1,6 +1,6 @@ # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." -include "/home/ubuntu/buzz/src/include/vec2.bzz" +include "vec2.bzz" #################################################################################################### # Updater related # This should be here for the updater to work, changing position of code will crash the updater diff --git a/script/teststig.bdb b/script/teststig.bdb index cc42d708f7a9fee87df7cfb0952c29a7bd454329..23a4e60887dfb3d47ad425f73d7594795d4bee5c 100644 GIT binary patch literal 27349 zcmb`PZ*14q6~=#xs8BYkPU;*?XwAUQ(oG@Sx&tkyZa63Fl z=iKMqdw)$oI-*vaRr}vMs{EW-Yq{pSMZGQadY1LJbWQ7?J)^s2asS-UKTcXYFwnAi zZhznPOIrGVICtUFdA%*mdgq=oe$K$aiovzoR}Yi=*8eB9q<8U>#Y_6;e=c+Dpjxe? z$P9}`bSIK)ED|Tgq#6;`#3FH0OzIzyy0S=oAtv>Glx#?m_)7U{ARmFsHI!c;?aRzO zvQ{ITx*}utQd1u8e+us5v8bjA?yvcr8U#1n)2P~Z;BG2WjEW`pBoeDH5(utZMns45 z%;XPw$!(G3j`oxL5x^wa+b6<^sLf6CM`1L&BxXvj#wO_~5?_o--6Wg>xbFd{Q%fZH zSS>TTHIm%%F{yhI$~TGx+NxS+R9B&TbW83%Q)(O%JS!)ve?f}FDd?8d0f@sX;FcrN zPaIB-B}v1ZPRnET?>t5q3!|}qa-)P1H;uvQ>%s_%)iPW1!#qY?!H64#{bVHfnHVo+ z%;cUI;~f*@bP$|o7l|P9ZNh12ku*5{1YvN_1^l}PM!Z-s8WZ|+r^WP^BD~mGYPiip9D8gB_^Zw;!9tORqhV)C05SprMvSY zS}hUrGH*omOYx-O)LkH+bYcwmJ$anQ=J?VB!if!Fw&gRw1q zE>*+FYMD~wfSfm{z!3jA!kAJbp?{kZeF`a_;)0xd#CU1I#=hH;W4uGrOFcP^c8l@y zs$=5b0NkA=D&$!$Q);Jh;(gb^U5DiQaw6K8m)u(M@WAxlBoRR|g8R2(bHPD=gV-G1 zz-)q6BuJa%o-?JIbG&IXII*+&(~PK1l4FOPQb!^b+FX#+)sUhDe;WI4yx`{X!jRb` z-ZU=O;e&)x5chmx#79{pxtAcr_Y+2B$PCZ3xV?F}cOf|%3BAck?k@Z;A3Xw2Suczj zjk(ue&tr7I_;>JhvR*g^dg)QvTXWfG_*gBI(=u?P{qv_O^@bonEf&>9dH($vq$ZST z(CpPR6I`Ch>fF4kawd9;l?!C;hGbq za=f$}GAr|Ne>V^JkUZRr1UFwU3{EdYCSlf%0B+t@xZlh^8i3T7izfy5+$2=)>>Q~F zA;s%q@Gppl0{K-XDyUY=WVKtW7kE+K0M0zA&;^WMYLF)2)-@A*3sOymO>k`1fl*J9 zf}K~(jA|n|opP9z!Rg7o>HBsbr(xi9ZAmV8qT2*ctR9gYoI3Hl2&!6~EN11l2=bt* zyaVK?mZ+GV8C5Gn;VFd)8k{ymhOb|Y$dKt0+`Jl^iR}ZU({n19o)WC#Z-d+v?8Z+mkDwdv05glG2pbINI*3r^@Ny8@Gps0f)lHT z0IOxR=9`dVXH(9QX+#LTR0O^f9f4?Qag4^ST7yV2K~rkGSX^+=wdWYh9l*`=Kro{B zA;sfYuq8Xi1!%q|@($s|sEoF}Qd}T7=T-~yU_TuYnNi)8 z<4ya-o0y!@1hb_Hf+v)*s2pwXSl@J?&O?2(poXeD27aFx(MT~~Ze6pFcA#=?B`JKY zmMQg2UV=Lj%A^tva*af_6;X8+iQp;adpVA|R%|ZlO$Wv1M#MPH04H8r=>%pYyqjYx z7XddXFipkqrg`E`!Ng7%Z=!!25v`L3;2VmG`qI3rtrD9HoGQ_Ete1NbWtzaM$BwwG2j=15DBgXv|~uQr-xdi3HmV zA96CA;8{pIy*~Y=llh&ZkhrT&@M@!#&DO?hF|Tf%Cv>>^n9=zm;}8+YWw5JX7JFmG%&2yY6nn^&+K}e}{}Bh^|M)PY znj;Pn98i~t1MscM=&9@S9N;W*048Tdv@x%8GlUVhu@TYr!id|~Y=BL9$*mJ_;uENm z+(+U~bOJ-B6^XG8S$vbxW=Js-?3ZepQZI;Oa;lh8&El9NV^Ou`vAPdZ^dxj?gw>0} zD)_!TURaHeMYTspLhyay9KjwOpTB@qQ%Q_nZ}!z7VCUBl!M>UgsrC|8Fu|iFs^Gsi zngz>bVmu!h2D4O5m@+2v^=MCygMf1$f(*}?^eZE=Ws+F%&~vvW#w~0_^ob-!b1`K8 zj2_}`oNrEM#m+=TJg$z38OmRQdrFbO$7-2TohOFEJ~E{q$TO6M;sU{$_a*>MD~wQq z2&-R7g2AYU=7#1aH%B6(u^8LBuwQ zSMzYU=HcF#XS|d1Sp7~sJm{AFz}{Y3GI%z4Ew6%Yc@_MdR1i&HEpzWpkqQPK;UTGD zP-+jbcbCNQF_PdpdD!R6%;Uw(Fqm6VJ)V4ngK96}OfP(bQr`xr1vygJ2qz9KgVT?Z zQDcb+!Bxxb@_KMe8lkU4ivL107}a<1`@C)Pr@`rd$neI=zA|J6=Zw$e1vR5GtM*17 z>Z2vOz@L^aM?~$V6@wP{7fBAmMY?-GHVSW2>?0HTR2c_+b}*$LmvIpMqlp!H*xwUY zytkMMuFQ*SkvJv~KgJfmEzdCr@{*gBV{;GY*xc>HD7fz$z=%^N*x@%qs-w6-Fskzf zH#^+urC;aZu0uAwlm&*O!2#8elJUC(4s)Y=-H3>%Tp)`K@}G(~1qbR3T@v;1AGE?Bt>rE=WDM&%w6#|)gq>!ks> zg^g$~k*d+ZP1KJ{V%)k$Ll3R}7Wwio^`(H+= BguDO% literal 9585 zcmb_gJ#39(6#l4It5w>n(jTcyZb|6pr%Oo?Q4%4gH&KHUy}i*ZsW!b|CGMbIT7w$Y zz@P@LfrZ6l!NOu;VPUbbu&@|#T<=Mq`8?-a?p@v|&(C?!d){~3k|?T-{(tH4yEBT1 zM@Mq;(D}=`xUV;Ju0InO#h%n4chOvmItmU{;6aRd$$L0JFjlGvH(KE+313fyF!QqRxUSEki>U z?-n+iko+xu2AD2cTN9R7*kSGnjMmL6JIo(}QBMeqvpyC_0@D=4EVqjq5}1~-ToW@S z)eT9lw6mG_N$MWJXz&J6t?|x@I`^C3oOd* zY^r@M9tDeuJ#`2yUSk(k1EPq)$3PSjcn^po=9`B;fjrqefjh-a>2`nZ8KjFjXq`+wP3bE2>sGFyd6j3*m zK1sFuSUe>#ZNZaec5m8%O@kfgluuI6ASq(n>IXJuc2Uzl7N>kHo&h$*$?)9A;!VP4 zU9gtuerF&l;_C52V6dOi_5{57yqj|>*ygusa%-U)&0S#Dr62W+bCJG@N1QBU-V z)xahOQN%OQFa)lbMFX2R5V*e78endN#e}Fzu$UNVyM)d95H!&!s)UX1C8DK|_$2k# zC#g%oMvpy1;O~HqKHM2#?)ubCHv~@1e)GVFfTl%aGK2pTPHm zD57m8AWC<-LDUC<(YJJB6u9qGHx0r@>xMX|7KF`)(ES9l^clQ407((IyG4OX@oXAJ zi5|K{FTNr$Iwlcq>!ZLp)Xf9&Ce3^E3D~GdhQNCuaJ_;UV48iBy6ThEVGyN>8AP>! zD59lL`iS~0Fe-+q;v?dX&V0n4ni4iT^XYtSu(%y8Ci=}6A8(HNc(V~i5lKyh#l#N( z3f{DazGF!0ICw*Bh8Yk=l;1Uh(P51!zqG)pwM0_iz~Xkh#eHBgar5l}QF_oA0{;%8 aQg%@p5T%E(fz7P2aR~ed!06OvVDlFoI_WL| diff --git a/script/teststig.bo b/script/teststig.bo index 63e7db39574f1a5b89cc74026439d9ba2fbefdaa..8619381e795ca18e490f622ae33574d28ea6ae28 100644 GIT binary patch literal 1438 zcmaJ=+j7!S6x|h5pw>&h1xS=#q1Nk*Z#v_PKhPP=o6d9sITNUmU{0W=pW^8FUB3F} zU%2);ff8bU$YN#fwJ&Gq@R}w;Hl(>4yrEc~(K#*TcS?gqlODx1R&fM_{+ebK#L-w$ zkOeW!Njjk*3`x)Wqz3^4VUp2o8U~pP^WT$T@MTHkWEnh()hHUG4X~f2n)+!Hgo8k5 zG>S(Vq48OO>Vs6F7M&&-V@^67MH7`q3(9CZ!$UJ@YD(iIqGL&5J-Sfum_8nTIO-i! zZ>V@KSv(9h<1iPBSdG<0#hIo^p<76)(jcMIS(+?}?-SAf`|Csvc7=X6^oh_vhQ1W~ z-4Vkpp+AHYgObZzz-DXh#p7#`I&XnR8ClfnmE@YpKTGnC$iGYSuE@KU&Du9b{#250 ziTtf3-xm2-Nxoxh zCf=3=zrIY7H^u``6#CjDAdw^G$9|ApdO2RwQ72dEP6R3KX+N)jp?R6O}H`XTc@E?8(~1`5syHlkmQG-CR8|I~s*ICyY-(ghR>Q!r-QrFj+} z%dEgNOr527q$&2vP*;sm^s# z8>E`k64dz&uc)-)YobkEl?zZ4z;(L;__QVT*3y>HFH1wA9~)x06#6L?#1zB^mRUd~ zcfUcKd?@l517ctA5Yj#)9WW?zb{V{~A2RRw z)}!c%F997hlIY98KgAQ)UEXq;Q!&z+^ql|dadKN^$yap27}y7fq?kwY&q8uOXYs{C vKMA4Rkvc3Z%Ud_^oJw65I!y}ruv5Bg_7~5;`^)O?XRgVY=euNwp67i78Yx+$ diff --git a/script/teststig.bzz b/script/teststig.bzz index 0ef792e..4ae66b1 100644 --- a/script/teststig.bzz +++ b/script/teststig.bzz @@ -1,3 +1,4 @@ +include "vec2.bzz" #################################################################################################### # Updater related # This should be here for the updater to work, changing position of code will crash the updater @@ -12,8 +13,8 @@ function init(){ s = swarm.create(1) s.join() v = stigmergy.create(5) - #t= {} - #v.put("p",t) + t= {} + v.put("p",t) v.put("u",1) } @@ -26,7 +27,7 @@ function step() { #v.put("p",tmp) v.put("u",2) } - #log(v.get("p")) + log(v.get("p")) } # Executed once when the robot (or the simulator) is reset. diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index e2e11f9..65f438b 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -271,7 +271,7 @@ namespace rosbzz_node{ //bzzfile_in_compile.str(""); std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); name = name.substr(0,name.find_last_of(".")); - bzzfile_in_compile << "bzzc -I " << path << "script/include/"; //<<" "< Date: Tue, 9 May 2017 14:33:35 -0400 Subject: [PATCH 11/26] fix launch files --- launch/rosbuzzm100.launch | 28 ++++++++++++---------------- script/testalone.bzz | 2 +- script/testflockfev.bdb | Bin 0 -> 66750 bytes script/testflockfev.bo | Bin 0 -> 3676 bytes script/testflockfev.bzz | 3 +-- script/teststig.bdb | Bin 27349 -> 28201 bytes script/teststig.bo | Bin 1438 -> 1478 bytes script/teststig.bzz | 4 ++-- 8 files changed, 16 insertions(+), 21 deletions(-) create mode 100644 script/testflockfev.bdb create mode 100644 script/testflockfev.bo diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch index 01d9402..69da82f 100644 --- a/launch/rosbuzzm100.launch +++ b/launch/rosbuzzm100.launch @@ -1,20 +1,16 @@ - - - - - - - - - - - - - - - - + + + + + + + + + + + + diff --git a/script/testalone.bzz b/script/testalone.bzz index 086de53..938b743 100644 --- a/script/testalone.bzz +++ b/script/testalone.bzz @@ -1,7 +1,7 @@ # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." -include "/home/ubuntu/buzz/src/include/vec2.bzz" +include "vec2.bzz" #################################################################################################### # Updater related # This should be here for the updater to work, changing position of code will crash the updater diff --git a/script/testflockfev.bdb b/script/testflockfev.bdb new file mode 100644 index 0000000000000000000000000000000000000000..456da2a0f5b684005b70cfe8b75dbf429ae7240b GIT binary patch literal 66750 zcmb`PXPB1N(Z?UfC>B5k6;NSu7jO$KA_xI7^bQJ3qM%Y-b`y#sJtCMFkfKO=qZkFF zOQc9s%Bx}lG!&!QVnQ@xHzD?tSfYaD?|Emg%r&3xIdjgJXTRM4Yi7=G+WkDR>6lz@ zaPI%lsr1=8SNe-_S67sd95JP$bl}Bh!!9i=oj9TVhd(<{o;Iy?V)=wo*G?*(R55W< z<=AoMV=61AG{0)vv^qb^1>nI4vvLXBa%4NC-owv3Q3}uPipw_x!lzxfj`5Eyun*e$mQmc#OXe%K_}*NzafdE zd{Sqfl*_Fq38;n>>0r+qx!f+2a9Y(5i8G3%jA7TbZ-&$mk^=bQL^}97p8qjPRI^EG zt5zMG%Z(<9(|l5e;A#d*S2<)kUk@=>Myu5 zI;d~(4ZzTjtTEEArBC;k2 zAT^Anz(qKbRy~GTSCK>>eMU94_tBetZQ>nOU)3p*Y`=iEojG|;baWEwyK8M zyt>$=G*{!r=H=|p`^Dzf&u7(9pnW(=7)LejU3&7HT#INx8@5-@c2Ian%S zDDX*LE^g3SujU~E*ON7zi;*|D6*stzq)-LJiInOo)+M7=x-x7K>yooOpPUYcCva_5 z6r())jEvQM1ZV?EI7hX*VlgT5S@k(;JFnqJ81xOEg0S<5=Il5f(yah1(yajBpfW6= za2sQ#o7a=zYDF3tJ_c93@baV_iDVpUg|LN_(JIYV15DWL81oe=H49QDWR3B= z32oIuNO7mZWqKSu6^X-xBWklnN^>;@QjJI=-zW7dqzXx*pHJ!{kR9CPaB>Ve6i!C2wA5%wT}=|smjlm3ieC;GY1dYLivZn1RykwU1Oej3#QAdI6G(Br zGUlLmu)e5QCFE%24K7B#f??rcEDa~4l&%bq;d$P1kV6(J?O?-nrSE`9=l(vc#sJZA zB;joIF9_P5!}f20_#Cpzxp_4O32R8oSljgtmZ6~UCMhQjO9Z0eBAiG>F9V`uNWyr7 zn)bYOh>ise6UZuOy=sN$d82o7bqA#8rm<=`VB(G5*(L@Ft`N3xGQyw{-3BS%2c5JJ z23Kg%;W%mE4k;KGj?pTOXd@uXDr5Vi-ch>Zg54Kuz!tg z;gy5QGr$#pp>CWRGz@R2Lv%d6;&q7~ zjVvIFW&%-XtOlcF?Ix>?Txl4#Ly9*qXBE9&(2g4j zCu6KM?L#rDak%+2qm*{=2GHJwtZ_=UOSj|PEf96qt4ci28@=)8-#S*m2A3NsR>oml zOAP^6dq@hG8BXL4?huIb4jm~~Ce|h60I3muSgcDt-e$B)OSKnVIV;1@1q^5S!oCku zD@ei#!>xiV`-1zB6w z6ZNh+N!6jxj8^HeKPzBx-e3W|O0-QJ1`Pa9#f(Kp$Er}=pfl_XAyq=*cJ4Tz3mA-H z*AAYIKFAxrQA)$mOK{~Z$QvP5MzM14IHzC|8bMOVbk+`jgL<`@q?|KDV?jG|D4fU~ zT$GNh1_B0SAJnlr2&p1+%$PV@>V5%(6VWMRF>ww>uZqQ_x$g##1py05!q_HstXcxm zH6-P9aJiVEC%h($U$eLpAAHL1u#0O!#))-OeCwI8ct+79}rx1 z^hw<-wh3qAoG-X?{wRGI5Z^y9Mn$vPC-hG-!ufIL4t!!!S4&JB|~EI|WzHdUZ5zFwu_l zOgb2v194t>jU7jG^%bPL5~9XgR7(v>hv<%Ui1x=s#DgP$?H`%Wr=w#f+HqbIv^x>~ z9<(P~mx|Cw`Q-o%3@0*H)kNYL3$o^FpGX{McP*;y5IN}le(*#fzK|T`E*(kx5s`z=v9=ho-a%IJ zG8bu86}aS^b1rdL0MS8Yl~d{oNR3G&RUl|DAj!yB&4*MIl5lRRABn_qez86sF!93c zlzIzN{L6ukzF3`&Z7DTCkRAw1K~u*>Ta=yJ7e`sIz+z%qFu=`<1DHX9VQTkswGxhs!VLD zJikYV{bk(X3bMv2b&t3~=dis`++cw%RvOXjXny=o#@M`c*k6Pc3=7A&hHI%-ARwQl zjIUU=)S+~U4ia2Bb8uU_CGIR-+X#x4ldD$*?am)s9|IEjMT&C;Xd#w3r_}L+cH=Cn zxjLMVt3tF?E-ub={v1+81cR|4>&oy55RKeYwba?^5SU8VU zH9(XLG14)dNUOe2hv?pPJ5G_Hz0l|2KLzd1#pF@Yeg#>>;g1Y^13b?KxslJoS&&LZ z`$0(YD+}j7I2yHm4>^|Sb8wAF9MBq0#<1&xJPwJ&FX)_YVmVCV_YBVQ;|Qd>kz-B= zpAtD}Y+kw`FGk`tC98O3%NTYoH5*(_A}QQJIFVBS5Qq-&N%cX1ZXk&~pVR}8nwv(d zHVQhgmd+cT3aPuuD&xxmeS>2}Iy>vtT_SOe1zD%_NU;wZW2L1wi5qk}xDqqNMhZJR zMmUkQzlg9ul16Gj5|7i^xq1BdwP23=_;l}2rY5$9W!T24$mTD(paB?*SFtnj?ALR>s=X7)X34mb> zSp(H@B4KzLVdwF~=%5aJshE(9d{%v(ZbH5sH`t4;L8A^QGFCewm1x+`hbdQ)HO?FC zg6HoeDHs?|q=Os8SZj=xhGCN6%2{MyffWB541Zc2)K(pX?%a(ej9aRfdRC0JwQW}E zDten3Yx&#D$Q%5(NF4s}5+kLa5VRX78SUVFz?5j%{s>aM5B}5__DkSZqG7vkx(RuD zx(WGOw23_wBj*6QQB(#a3_5ZCnl7DJpwaWf%M~Ic?b@nQVs3XJWM z|CX$AcIUNd^c&MijYq8al9aKfY6tg2iuXa|3ZSK46_vq=s7~ik(joeg;Huci)jNP8 z(LT6NzyM#viKM-q*aw~CM{V3-53ZXldU+Rp_Hym|5B8!7df;ObJJ zRXxQL=X||$vf#?;;B_E?C5&Gv=-PffvZWiv2)>4sG3=VF)`F{cHYsh@VMrB`gz-(0 zmf9oMCEP$b86DJ8g#uA03|qw9Zj{ntp9UECl|2mfIJg?MJ<;5L4Y=YT#2Uk{9efE= zTwIJDN8jKwa8*b#a?b7F3q+k(?G=bRN3|jlP)-hFJ_;w2_C=85mjlKnPKW(xFeTA8 z(EudycV5QC(N@iZRH7AN3J~8*;Wl<>ZPlxgN;J2>2~+syIDs<4pmT6dy2>z4R0bpM z+NwW^%Fx6|`*KkkIA$4RrEl<3LA&#ds>dKTmmDkcIoMDnj`PLuY>_z5j`M(6my9sz zuul~^2wKC5yukzMahvU8;xD zmuOw;mhJ{C5THI3D-3|)M27uKB;GGc0yp51+Ag?i=9B7|Ze2PJUhx{vyG~@RIti`{ zd=AzZTp5duPUr4|t2~=kTB;`cU^kL5#!5@ILm%Xq1I{I`614XvtAJQIkvG_1aOG@X zr-Q4lWDTmI$Et^s!#w&s->UrzTyY_GT2)KHV2qW9p}l~ii!WBy07EfJINQWNJkMYE zJ724bSs9Yh0JK)CSb6He`*{s!2eUUk(^= zP+Rp1q&Q)k*kYvz$SSb{a9U>!yOw%ZAZiS|mby)F<^1Kq6{y0zh8t&5ZB+qc&TDub zU)bkEiq~)>4BD#y6WfF{?9<3s=gCkIXh07*d};5ioC%y(`^$y(s7k1xIzwvlhHwagEvBoCu8T25bA)dZe)$| zmzmnCj_5eNr8*ar>Oi!Ztiqq+WQ>)r46T4@IZ1HLBBcfiM4jWuSpre0dK~O25apOf zI=H=BvK?m*5amVJ*(TnH6jub}0I3n}BoOWD3;Xc`QDd9Xi2e~$ygM5m)Kaa}(S9eS z_^ZZ-zE~B4fJBSQLP+sq;*8bIblb!ZkvPUlMkD%xpq&#aV>)Z8H_~krtJ7^0FM;+e zi6>`exB!TbBq?VOE)%rp`Dni?-MUniE^!t?>h3hLdK4Wi(dIQvBQ&Ou1=5@$>t zT}8Jdhxx6Vb5wg1Imiv(_&Qo!wMOJ%6Ca}G@M^8v&kxBth9rF!1GKyr**~~)KXsvL~#S*L`pTr%)qZKjPI}X%rHP8YAiBZ zsvf-RMvfV8P)m&$TsarsqXbvHtwx4@FbKGh9CS)G68oStar$8&+(}kB5xp5)aVv#^ z;Y7x&emWSsqYrYpyZWrE39gDs!s*};0fX};`8fgxC+)kc{dc1`PRKe3KLrfDh8q#p zQfH=vp@)E>w-1I|0tVzzIFYo!3@P4moLsdRJC3t?-IH#|xfRWew+XHgk+GVG0PUr4 zJ6C`YA;l|zvE%6OyjvuWG3=W5&qU(zoRu+FT521*GcP8_SyW4%g&gcoVTZ5bL|U~0 zIao*%O>9y+>{UQ?GD#q8;bgQ*OSM3N5^WPtptkeEYYe-#YCaHsiX3xpULOhCoo(W5 zz|bSkD$UhQNbzD~jFp!9JES;aoWHWzC=hK*jz)4d3gv*e3FmzE9Hi!vHO7{zxjIkK zZcJw_^$n!%C990vgqC^|0eXO>j4)`an?U=+X{3r!Sq71mF$XoGKNIVckt;3L0JIMv ztBj>jOHIM}(U+unQjVnkSRl%4xKrwRLA$eF4NSM=6eHGGqzU_CNbw64QpCU9EB(c| zt1C)Jj+jzWI`HDMVV9PbPMlEw!=IfePn%Xcv3$a)YbTXz+8d^WVXANo+z z**>XZs0?dK0tSW?X;o8HhOH#ww5m*S z7>rY&=4vkL6|VqJu0DWNq7`5ZN?#$xsu_Jo!mwJvU@XX*t836wd7E&)NVx!9@xlvV z!-;fo8KmwZ3H<4i`X0|G+9vj<+a?x)%S7A67<_1gUu><(&H3h{Qo=hZC7NM<*YJ$uTFQU!k(_=H)ChLxAYMIIeWqUjU*Ub|dW?hVRlLx(kT5 zAqSmf?Yja7XV}*R2L7s%(>h}g>agE);(ymAr&X5&h8NPna46jk?oM}ux1bO5%K;}0 z+fc8#_w(uHm?1MT752vGjPdeHcrlb9HNb#$%65kCzA`+*aPpShDT}l#A4JQ)O9|=UA zv_B^hb$-=&9uQ5mE?odbd3QECsJrtrNY$Zm8)s3SINw8xU)ckE%MEI)_Tu@rB;k}Q z0`0sm;W9l|ErQf=lE8cvPDVtvgR@XN`;r9DSsA5t*iTKjE{#Tj){<4mma4643ECed zDI=mzJ`-ARyypibOI?c6$s4_M z^J)l08Oy4a??H-R4me@xD-d;7(M^y_v|6sjwedG`&I$QYI@vj zgF}!wjpD4*Vc!9%LXvW>mebN9`Ua%tlU4XLoX8vO3@&-$btcXUczz{W<18{SL5knW za0!fbuo@8MebAVL8qs}_;(gE}X4h2Y9*RRMZKF@?Q|J|l0i zE@0sAQk^$=Enrwj)?j=HC(^360tOi9ky?tdk0uEiXpzz|G!|j!@JBj000F8bYn)-P zhX73{Dfk*rq*bqqVcU3vI#xF$R=vp@XX1PUDc-!CW9`6nVSflxym>YAg?$~GUlmCh z`=B0*?ndJ9ELz~R>S#1So<*HEctCJvOlQs27&Q74atywPlaY2UH7wmMx+@Ld^my1|AUk((DShXZaBNOLdILO;X6Q9&^Ab__Cql22O zx5PGKl+s)+L9BX{WBjjXBV+Xiqe2Dwi0 zXU4GW8?1pGPBhkzhg1p0$k|f&LyE^*V-?j7R)|^DNV`s)F9o7b+6RkS6o`eB@dmY3 z+aWcTB%D(3qqff{DbN~Dq*a?m;y8cTcN$DtMby;7vJvB>Dec}5J|U3^x}MdHjM z3F8fFt3H6#%_M~z2qzNJ{>VX|+nwn=4bQJ5Yn(aQ105^Tu)SUk+fJ+6r=xwJ;Hs!Z0yux^R38CaMd5Zf`gb73D}Zrs*Ecv@(C*}_HlF7-+&Dn$ z#F+}I5{i|vifX9?km5DmxlJ6GZUq=4Rsf@e`UZ!kOPpOIah&PA04;eA-JlT$?ck@7 ZT0l|=TR4&RstoCTJ4rYb=gf5J{2x|w_iDT+0VvT>b>#y!-+Gf_tAcNhVFfhn(57zsvb<=S;qomJ{5UZMU*0ZTFEp zmK{E~q~dj?(`ZZ6ZP%gD`N|K(Y1f;sIDylKvgfzNsaB=a-IPwnfuQOI(%q^$fm;oq zWvBA62ismxSZ=$G`h9GJb<^`ZvgvzHwc>OFv2&%Byj{+%+68-KIWJ2arFA=J=f&PA zt>u@CmoJAZlE1!QSS_we^Fg`g)Z2}qTXp$@OajlBYNLZND~R?RwwQ4cwYs$=}MYz)Q>765p+MVNmh9ZRFu>ms{Sp8+dXlS1J|qrSgq+yRaggj_)^I zzua+yK7Qmh`Y-U@sJC3dz9SXi#U_0%yj6E(s~bq8DmQZlyKEP(<%=7(1dWz^Q|j;- z?nNo8{{$I1?JD$9-NN!}UIOQ#>(y!~!qq&B(e&zKgDci*=atl&sA!;b>vp8%Hl3ic z?W!%W(+C<~8yY<*PBSniD-|yl?RCk8@6f5V&_1N!+iNwqwk)fN12Xa^Bvf25A&(r4 z!2=J2(g|GDuu`Tuerj>iJkE;kDQ!dJx`&Dq(2h?uim-`(xoz3<8z}yY7pRkRT285& z@3oZgZGy%!g^ff;>CnY0IFk}oi(h=B~;SD zr+_B`(=1LxohAHT!$m^;B(d+5~+^N1MJ8E1R7bhql z%E`n)dTJ;S6NKS+m0@6_%a$B}hM_Jbr2k_+@m%wXXPXZfeh}~^Od=)Gtum1g6_~l1 z5GTh`u$S>_saLQj6=%-U<7SzH#^(D2GIdBh)Ei8tx$D6wL9YiVCi@*=qmv590V{71 z%Y=j31;Kf>^r%{CZjdrUj%9LJCygH-E;D;NpuWy);ME)!D?ASw;6#|yJdG6ZEXz!R zI-wawh6`5Xp1~&>8qXGv2j5d>Jwc7o;R0Y%pPiImk7mzd&=lCdC!T9Fc<}Vig;B#P z<5p#96z!JYM>o@G#1xaue2X0t>J313nkMQ^tVw;g!>kf9#S7Lr(}nCgpp~WG)&^&Y zbs2`G@9dJM%)Rn1XtS-Q-lK|1TL(OojpH!un8p%jLug29AxE0`i={6dYbL81hB21V z=_uax=+)p7($E;`=*E$b?kc|&y{?`E2Cr|t^TbAlUl6E##E1GAtNKL4PqBt`6bdg; zcu@;;5#et2cvMHP64oGK6U!zfhK^k5HR4kqYy^3_M&!<9r4D9(j+Z7G-$o;)k>UGRBc$NQA*$0qiAo z2-k14anZJ#X2sNJ4CX40%$%Bm(`h8M&{C_AhST)SMw?LAv?fav&;G{EO~OSG;dvhD z>7vgLjYVWyMp7KqdtniGOISa%A(i9VEBMTZ8(tr5Xd?K0K#KLX-c{BH$nTavH8wJzgh=Q*6u)8zVj#N6L Qt;;y9&An8lgt1ucKgmm=3jhEB literal 0 HcmV?d00001 diff --git a/script/testflockfev.bzz b/script/testflockfev.bzz index 1845eb9..463ca52 100644 --- a/script/testflockfev.bzz +++ b/script/testflockfev.bzz @@ -1,6 +1,6 @@ # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." -include "/home/ubuntu/buzz/src/include/vec2.bzz" +include "vec2.bzz" #################################################################################################### # Updater related # This should be here for the updater to work, changing position of code will crash the updater @@ -145,7 +145,6 @@ function land() { # Executed once at init time. function init() { s = swarm.create(1) -# s.select(1) s.join() statef=idle CURSTATE = "IDLE" diff --git a/script/teststig.bdb b/script/teststig.bdb index 23a4e60887dfb3d47ad425f73d7594795d4bee5c..4b9426d3872517d0d321201be6e7a14b09d6f652 100644 GIT binary patch literal 28201 zcmb_lYmAlE6}<`-vBIEbY-Ma6K!+N@5n2j;fK_LpKA}8)AOjAg4u~Q%kal>O6q<-l zK_i-gR#1a)YsDJH2P74nYST*7NZNw0)Y3GfO{o@ti0Sg{S;-2&nzi@&bMK$Kve(+{ ztbM+7@4&4`)oMd(|66^_>zG>ewKpv4Xr4cBSx58K$6F2(*D`obC>q?G0I31+>wc`qm*|p2^x;7YH8&j&G5zMOo z0jN0zDrkU<#Q>DvFu;?58Bi)Zw!9iJ_2Q#be3;9nYe1})DfKoX#*JbSI|NHkFO}eJ zq@tZ5H?%;oV-NsUp0kcZFc9IX%CJ7Ks$YFV1`=@k`X=!?Cnslwn*Qr6e zHpGY7C7JZ)0%H<;HYbw~=Vj8%dE~YWxnq2}KNWIlV6{xSe=X!1V^Hhh?$!d)KL&Ld z2%c6T@U~he!Szzri7}|vIoWhK7!5C#zz@|j$<0Uic$VZKH=#yAr?ELzeV7+>mqH*8 zr$Ed#=EU3@!0=egs*D*oGEeG{@}#~@q#jm`V2~RuQgaI%FlUNJK&+PO?EmCxv;!J( zp9Kui3)_|lX2O`{ek=JsIHuDf=+ssqf|BnPo%jT9n4q`lL>N=24ItNA$OVt__vYny zXI_4Tqw`E3$A3Ngo1Tu|-~3EqG$U&#sH z{;=e{9F6u!@bXc|l=~iZ?)#-G@T`^z^{VK^+d;!ibs*Q7Q_P0pzB$`DzlIAR5u(Ce$$~g)tYDdjX&b z!B<14(b6>@b_|%kl1*QYZInKuQBdxMqETSZy#^TGpRgiBqk(x5w=Ylb-5|$(#)vb> z)eAYon49E{JaStlzk|n$Z3vW+#o2gUEt6mm2r{?%YC^p!TAdNAYLf&nAI1zLOwJ45 zrvUZ!LJBUemZ{V7Jju_?8wX=^!gq>D9^88?@^os+)9HA0pJS5&Y#L!Ppg1lAE*~eI z<3p>lSytxB-I^zNzdX5b6uEf;V(9cbU=oJZAjr*!3?6n&AFTq^$t9D5du}2+H!cV2 z5kT>|F8D*F{*ZibsS2poGPT+x-3wfWHc0ol=gf-!3aFulRB&w8f!w?Tg&eMyS=DA3 ziC=~Uvgz5pvH5PEP6MFRf}&6GD7FPUeWgGFwOU4~%#g9kkzjUiyGR~PgTF!Y(+a7e zpT?sUo-awz&}pXV#HT1zr~M!|qL5=nCe$)?o)En2HONg6jY!UbnFWm)xU{56Y&A6E zWC?DR3lZF$EWyNl0#KYR)X5~c7XopZa|#=hH$o>4^WdTI7ebDendJ5fIZAGjs|O6z zlD;x4+M1J=J0#-RHM61zq@uuiV5d|>sm+Ss%gd$Z=v+cBohP|OOBz0!otH~DOD^#? z%#{0p$Q=xmDH3tPv|N$Xxhq9-G_hKyPNzesg#`kr5vZpn;)1^yM7$SkML{HyVf93BxpiCnv;~fBq@Vi_j@_H^q3F}bnT~TKF>-) zV*dgZkEX#5GD9+nqrf!4`64%~GB(Lol1ahgy-p+#y8EC=&dVWFrnMkp46*FYM{D9>4m(C8f6S{>zY1#1)cj= zA%(ZqGNE3~Blx6bIQ?YU@>MW8w~&Hl5pp{~Zfb!*aS^CXa)Ni8XvE{Gq0#LUy1}G8 zB%#YcRW!YH1?1-S0&~fPdM_v9CPQwX2LhJ7R+2JE>?}!2P7}ib8^i#-?J}jFo!7OS zC3FL)xC4M;LNeYAjYdHuR>4D#0dpnv`DO_Zyp4p&=^)03ir}_gD^dr@y$=}fdc3Wc zq0t-&lQfW;@-%uaZ<4fuU`x@5I+^?JB|vc~aRiu9!$d2_oC)=e45q-}73>ztfmkiG zqEBQn1tV^5UiYTwOq4~?iPs8@I8&#MG79+ZjtO;GBp(#(-aL^!XoMLeIq$xWjJh5= z9~pI~PT!Ep5;#_@!}g^K0vUb~Fh>$I^jQxW?tDJW88H8rYS?%K<~Nd(Y`WR45vYh$ zH|XjYz=+>8Q!9hq>ynaeyh-jwsfb5YqoO@h5glz-bWdI$9Ssxk4vzn*i&@pxl1IU* zd%5Hhuk#GayYlkr+mc5lXH>K~uX9(3MuCmeEgAtaa-7(bM{a{8C2yGxxj&YqWG)#n z<3WrzWb@5Uaxb7*3HD1w?%zp%bHequS^yWD6@NKOZS3jg8KnAwCKalV^-A%-Q$o7+H)bGT1r(xf-j+arxqyM z(+HPig|B7@5Aj(ZnuABMjo5RXae;Ag7%;4eIvM)B41I>h z8{Z@P1P_FpL?6bRLF`k}hrw&W{1J}k^$RZ<&7D6M74gAhaFN`EdK+?2I-+YPREwk} z?Px+hl$Vl=CE9}Lfp?(Qf!xUABcMk7v{|B#cX0-}UeJi6AP{xe0EQpuj5Dek0DZ<3U4onB zPk>_h^4l-7Yu}OZ9TMx>7U>$V(G8t?LvCJF^Lr-)=KQ?;{&Sw(8oXK;|ryO!9;jMIv9l73)!a?Vt5-Ncy^xb zYal!GJUFQKfz|XvEC}^o5L}o8bzRQD`Y{*{DHWl(YMJ(|hfYbe?+rllzdHz4bq+qy zfzDTB(rg6`A6)4x1Ez1z+*>5g=M$$<%}LTRk~5qCW?sjR6O95tkX(+6T8g8C5%-pm zLvfK!asc~{4`j5XDftu`2fPF^p?)djAoydO6?wA1FIv&%Cc%|?Ro#-8n5*)tnwS%F zkL1MM63HiC85{ZZ6OlT&?;0RAhf~nuHvy`(q(HE$Nzyes+|cRKoUYY@4Ijz^Gp-9T z{5YS14N|8_$5@rQ``-l=j}w&HgnA58^KyvIH^|M%%kJ%x-9eoWO5l>Dsnbj$NHdsF S&k4ccoSOwGS`u6%gZ#hZyKBS% literal 27349 zcmb`PZ*14q6~=#xs8BYkPU;*?XwAUQ(oG@Sx&tkyZa63Fl z=iKMqdw)$oI-*vaRr}vMs{EW-Yq{pSMZGQadY1LJbWQ7?J)^s2asS-UKTcXYFwnAi zZhznPOIrGVICtUFdA%*mdgq=oe$K$aiovzoR}Yi=*8eB9q<8U>#Y_6;e=c+Dpjxe? z$P9}`bSIK)ED|Tgq#6;`#3FH0OzIzyy0S=oAtv>Glx#?m_)7U{ARmFsHI!c;?aRzO zvQ{ITx*}utQd1u8e+us5v8bjA?yvcr8U#1n)2P~Z;BG2WjEW`pBoeDH5(utZMns45 z%;XPw$!(G3j`oxL5x^wa+b6<^sLf6CM`1L&BxXvj#wO_~5?_o--6Wg>xbFd{Q%fZH zSS>TTHIm%%F{yhI$~TGx+NxS+R9B&TbW83%Q)(O%JS!)ve?f}FDd?8d0f@sX;FcrN zPaIB-B}v1ZPRnET?>t5q3!|}qa-)P1H;uvQ>%s_%)iPW1!#qY?!H64#{bVHfnHVo+ z%;cUI;~f*@bP$|o7l|P9ZNh12ku*5{1YvN_1^l}PM!Z-s8WZ|+r^WP^BD~mGYPiip9D8gB_^Zw;!9tORqhV)C05SprMvSY zS}hUrGH*omOYx-O)LkH+bYcwmJ$anQ=J?VB!if!Fw&gRw1q zE>*+FYMD~wfSfm{z!3jA!kAJbp?{kZeF`a_;)0xd#CU1I#=hH;W4uGrOFcP^c8l@y zs$=5b0NkA=D&$!$Q);Jh;(gb^U5DiQaw6K8m)u(M@WAxlBoRR|g8R2(bHPD=gV-G1 zz-)q6BuJa%o-?JIbG&IXII*+&(~PK1l4FOPQb!^b+FX#+)sUhDe;WI4yx`{X!jRb` z-ZU=O;e&)x5chmx#79{pxtAcr_Y+2B$PCZ3xV?F}cOf|%3BAck?k@Z;A3Xw2Suczj zjk(ue&tr7I_;>JhvR*g^dg)QvTXWfG_*gBI(=u?P{qv_O^@bonEf&>9dH($vq$ZST z(CpPR6I`Ch>fF4kawd9;l?!C;hGbq za=f$}GAr|Ne>V^JkUZRr1UFwU3{EdYCSlf%0B+t@xZlh^8i3T7izfy5+$2=)>>Q~F zA;s%q@Gppl0{K-XDyUY=WVKtW7kE+K0M0zA&;^WMYLF)2)-@A*3sOymO>k`1fl*J9 zf}K~(jA|n|opP9z!Rg7o>HBsbr(xi9ZAmV8qT2*ctR9gYoI3Hl2&!6~EN11l2=bt* zyaVK?mZ+GV8C5Gn;VFd)8k{ymhOb|Y$dKt0+`Jl^iR}ZU({n19o)WC#Z-d+v?8Z+mkDwdv05glG2pbINI*3r^@Ny8@Gps0f)lHT z0IOxR=9`dVXH(9QX+#LTR0O^f9f4?Qag4^ST7yV2K~rkGSX^+=wdWYh9l*`=Kro{B zA;sfYuq8Xi1!%q|@($s|sEoF}Qd}T7=T-~yU_TuYnNi)8 z<4ya-o0y!@1hb_Hf+v)*s2pwXSl@J?&O?2(poXeD27aFx(MT~~Ze6pFcA#=?B`JKY zmMQg2UV=Lj%A^tva*af_6;X8+iQp;adpVA|R%|ZlO$Wv1M#MPH04H8r=>%pYyqjYx z7XddXFipkqrg`E`!Ng7%Z=!!25v`L3;2VmG`qI3rtrD9HoGQ_Ete1NbWtzaM$BwwG2j=15DBgXv|~uQr-xdi3HmV zA96CA;8{pIy*~Y=llh&ZkhrT&@M@!#&DO?hF|Tf%Cv>>^n9=zm;}8+YWw5JX7JFmG%&2yY6nn^&+K}e}{}Bh^|M)PY znj;Pn98i~t1MscM=&9@S9N;W*048Tdv@x%8GlUVhu@TYr!id|~Y=BL9$*mJ_;uENm z+(+U~bOJ-B6^XG8S$vbxW=Js-?3ZepQZI;Oa;lh8&El9NV^Ou`vAPdZ^dxj?gw>0} zD)_!TURaHeMYTspLhyay9KjwOpTB@qQ%Q_nZ}!z7VCUBl!M>UgsrC|8Fu|iFs^Gsi zngz>bVmu!h2D4O5m@+2v^=MCygMf1$f(*}?^eZE=Ws+F%&~vvW#w~0_^ob-!b1`K8 zj2_}`oNrEM#m+=TJg$z38OmRQdrFbO$7-2TohOFEJ~E{q$TO6M;sU{$_a*>MD~wQq z2&-R7g2AYU=7#1aH%B6(u^8LBuwQ zSMzYU=HcF#XS|d1Sp7~sJm{AFz}{Y3GI%z4Ew6%Yc@_MdR1i&HEpzWpkqQPK;UTGD zP-+jbcbCNQF_PdpdD!R6%;Uw(Fqm6VJ)V4ngK96}OfP(bQr`xr1vygJ2qz9KgVT?Z zQDcb+!Bxxb@_KMe8lkU4ivL107}a<1`@C)Pr@`rd$neI=zA|J6=Zw$e1vR5GtM*17 z>Z2vOz@L^aM?~$V6@wP{7fBAmMY?-GHVSW2>?0HTR2c_+b}*$LmvIpMqlp!H*xwUY zytkMMuFQ*SkvJv~KgJfmEzdCr@{*gBV{;GY*xc>HD7fz$z=%^N*x@%qs-w6-Fskzf zH#^+urC;aZu0uAwlm&*O!2#8elJUC(4s)Y=-H3>%Tp)`K@}G(~1qbR3T@v;1AGE?Bt>rE=WDM&%w6#|)gq>!ks> zg^g$~k*d+ZP1KJ{V%)k$Ll3R}7Wwio^`(H+= BguDO% diff --git a/script/teststig.bo b/script/teststig.bo index 8619381e795ca18e490f622ae33574d28ea6ae28..d6597dd548eaa378639de69f9724ca89cea8f6e8 100644 GIT binary patch delta 286 zcmbQoeTr^^oq{${KvF_M21to3$OB1f1qC1p78jQX Mx`jbzatx~z0BDXK_W%F@ delta 245 zcmX@cJ&&7FcOv6|$D-8Y)Dni2)Z&t&{7MD|5e5bZMOzjI1_=dm5HkkMR01(yf|=SN z<{Va#0v!-@?qpX+6$wiw1_mhwAfG{8fe}bbD*#n6D2GgLWE7K_k0Hqgl6*9IJ)>AG r4>P(>UXWxohAd1<);%TUea{AmbhD diff --git a/script/teststig.bzz b/script/teststig.bzz index 4ae66b1..d3c4139 100644 --- a/script/teststig.bzz +++ b/script/teststig.bzz @@ -23,8 +23,8 @@ function step() { log("The vstig has ", v.size(), " elements") log(v.get("u")) if (id==1) { - #tmp = { .x=3 } - #v.put("p",tmp) + tmp = { } + v.put("p",tmp) v.put("u",2) } log(v.get("p")) From 6884504c8e012e9882f3c56fcedd3d73a9e03eea Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 9 May 2017 14:35:38 -0400 Subject: [PATCH 12/26] clean script --- .gitignore | 12 ++++++------ script/stand_by.bdb | Bin 7810 -> 0 bytes script/stand_by.bo | Bin 572 -> 0 bytes script/testflockfev.bdb | Bin 66750 -> 0 bytes script/testflockfev.bo | Bin 3676 -> 0 bytes script/teststig.bdb | Bin 28201 -> 0 bytes script/teststig.bo | Bin 1478 -> 0 bytes 7 files changed, 6 insertions(+), 6 deletions(-) delete mode 100644 script/stand_by.bdb delete mode 100644 script/stand_by.bo delete mode 100644 script/testflockfev.bdb delete mode 100644 script/testflockfev.bo delete mode 100644 script/teststig.bdb delete mode 100644 script/teststig.bo diff --git a/.gitignore b/.gitignore index ae47e75..64f22eb 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,7 @@ src/test* -.cproject -.project -.basm -.bo -.bdb -.bdbg +*.cproject +*.project +*.basm +*.bo +*.bdb +*.bdbg diff --git a/script/stand_by.bdb b/script/stand_by.bdb deleted file mode 100644 index de2da98de2aaa76c5668d25c4392924ef77479a1..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 7810 zcmb`MJ8X?{7{*VFUX)&Fi=Mhv(st1HYm?HXh@=Y^5=SByM=vLF37Vd^IEjVDf`x^J zg~h_cf`x^Jfx#t&NJJPIbg9JS_}+Nt=Y9W6zvX@UJiqsU`@S<*k;$}V{*5I4JCI2Z zoj6uVCPrrq$)4T0{JvaLoErc8I5JZzCB^Zn!;{lVaeDOlM1Jga-&m>C51Fd8e5o#T z88VG&d9^Mx02wQ{ze1=hj`m}j(CnjIc^k>5X$C@;|xDQ4R@TH`52jQLfoq(o+reP zZR;R~a>zW6>C|ru#% zD`78wMs8N>p%S<;CXS2A5o&mc?xq?t#>89+Y>twfPTfsSjGO0>u`S)vslk{|)xyo9 zl!@c}%?rrbF}FZRypcjVwyjCHvDw?LyQz<1^9nN7XgrpOFpGy{*fdZmV#2| zcaX7*tmCV|8OYf58`49y#f1786Dmt?9N&vSksHU!a27HFh|fXBUVv;-2-9yJv9T|; zg3K3UW0yBU<~)UR{C3wF6Y4vKvh7O<)dd+_l8$ZbLJXUWkg=Nt9@TB@Cu(iKS)+f>|e( zIFQ45c4lY2&IK*YEO9Dp&PkHKuBj7j2ilvTPyoG}UK*(+2Wvqa) z^z|cehP*90?*Kz@#FFpxF0h``^zBLC-~rh>9}HzH-WPc&nVR bvAnupJ7S03(G5=jb`C{|0{_Fq`tslxS&C1c diff --git a/script/testflockfev.bdb b/script/testflockfev.bdb deleted file mode 100644 index 456da2a0f5b684005b70cfe8b75dbf429ae7240b..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 66750 zcmb`PXPB1N(Z?UfC>B5k6;NSu7jO$KA_xI7^bQJ3qM%Y-b`y#sJtCMFkfKO=qZkFF zOQc9s%Bx}lG!&!QVnQ@xHzD?tSfYaD?|Emg%r&3xIdjgJXTRM4Yi7=G+WkDR>6lz@ zaPI%lsr1=8SNe-_S67sd95JP$bl}Bh!!9i=oj9TVhd(<{o;Iy?V)=wo*G?*(R55W< z<=AoMV=61AG{0)vv^qb^1>nI4vvLXBa%4NC-owv3Q3}uPipw_x!lzxfj`5Eyun*e$mQmc#OXe%K_}*NzafdE zd{Sqfl*_Fq38;n>>0r+qx!f+2a9Y(5i8G3%jA7TbZ-&$mk^=bQL^}97p8qjPRI^EG zt5zMG%Z(<9(|l5e;A#d*S2<)kUk@=>Myu5 zI;d~(4ZzTjtTEEArBC;k2 zAT^Anz(qKbRy~GTSCK>>eMU94_tBetZQ>nOU)3p*Y`=iEojG|;baWEwyK8M zyt>$=G*{!r=H=|p`^Dzf&u7(9pnW(=7)LejU3&7HT#INx8@5-@c2Ian%S zDDX*LE^g3SujU~E*ON7zi;*|D6*stzq)-LJiInOo)+M7=x-x7K>yooOpPUYcCva_5 z6r())jEvQM1ZV?EI7hX*VlgT5S@k(;JFnqJ81xOEg0S<5=Il5f(yah1(yajBpfW6= za2sQ#o7a=zYDF3tJ_c93@baV_iDVpUg|LN_(JIYV15DWL81oe=H49QDWR3B= z32oIuNO7mZWqKSu6^X-xBWklnN^>;@QjJI=-zW7dqzXx*pHJ!{kR9CPaB>Ve6i!C2wA5%wT}=|smjlm3ieC;GY1dYLivZn1RykwU1Oej3#QAdI6G(Br zGUlLmu)e5QCFE%24K7B#f??rcEDa~4l&%bq;d$P1kV6(J?O?-nrSE`9=l(vc#sJZA zB;joIF9_P5!}f20_#Cpzxp_4O32R8oSljgtmZ6~UCMhQjO9Z0eBAiG>F9V`uNWyr7 zn)bYOh>ise6UZuOy=sN$d82o7bqA#8rm<=`VB(G5*(L@Ft`N3xGQyw{-3BS%2c5JJ z23Kg%;W%mE4k;KGj?pTOXd@uXDr5Vi-ch>Zg54Kuz!tg z;gy5QGr$#pp>CWRGz@R2Lv%d6;&q7~ zjVvIFW&%-XtOlcF?Ix>?Txl4#Ly9*qXBE9&(2g4j zCu6KM?L#rDak%+2qm*{=2GHJwtZ_=UOSj|PEf96qt4ci28@=)8-#S*m2A3NsR>oml zOAP^6dq@hG8BXL4?huIb4jm~~Ce|h60I3muSgcDt-e$B)OSKnVIV;1@1q^5S!oCku zD@ei#!>xiV`-1zB6w z6ZNh+N!6jxj8^HeKPzBx-e3W|O0-QJ1`Pa9#f(Kp$Er}=pfl_XAyq=*cJ4Tz3mA-H z*AAYIKFAxrQA)$mOK{~Z$QvP5MzM14IHzC|8bMOVbk+`jgL<`@q?|KDV?jG|D4fU~ zT$GNh1_B0SAJnlr2&p1+%$PV@>V5%(6VWMRF>ww>uZqQ_x$g##1py05!q_HstXcxm zH6-P9aJiVEC%h($U$eLpAAHL1u#0O!#))-OeCwI8ct+79}rx1 z^hw<-wh3qAoG-X?{wRGI5Z^y9Mn$vPC-hG-!ufIL4t!!!S4&JB|~EI|WzHdUZ5zFwu_l zOgb2v194t>jU7jG^%bPL5~9XgR7(v>hv<%Ui1x=s#DgP$?H`%Wr=w#f+HqbIv^x>~ z9<(P~mx|Cw`Q-o%3@0*H)kNYL3$o^FpGX{McP*;y5IN}le(*#fzK|T`E*(kx5s`z=v9=ho-a%IJ zG8bu86}aS^b1rdL0MS8Yl~d{oNR3G&RUl|DAj!yB&4*MIl5lRRABn_qez86sF!93c zlzIzN{L6ukzF3`&Z7DTCkRAw1K~u*>Ta=yJ7e`sIz+z%qFu=`<1DHX9VQTkswGxhs!VLD zJikYV{bk(X3bMv2b&t3~=dis`++cw%RvOXjXny=o#@M`c*k6Pc3=7A&hHI%-ARwQl zjIUU=)S+~U4ia2Bb8uU_CGIR-+X#x4ldD$*?am)s9|IEjMT&C;Xd#w3r_}L+cH=Cn zxjLMVt3tF?E-ub={v1+81cR|4>&oy55RKeYwba?^5SU8VU zH9(XLG14)dNUOe2hv?pPJ5G_Hz0l|2KLzd1#pF@Yeg#>>;g1Y^13b?KxslJoS&&LZ z`$0(YD+}j7I2yHm4>^|Sb8wAF9MBq0#<1&xJPwJ&FX)_YVmVCV_YBVQ;|Qd>kz-B= zpAtD}Y+kw`FGk`tC98O3%NTYoH5*(_A}QQJIFVBS5Qq-&N%cX1ZXk&~pVR}8nwv(d zHVQhgmd+cT3aPuuD&xxmeS>2}Iy>vtT_SOe1zD%_NU;wZW2L1wi5qk}xDqqNMhZJR zMmUkQzlg9ul16Gj5|7i^xq1BdwP23=_;l}2rY5$9W!T24$mTD(paB?*SFtnj?ALR>s=X7)X34mb> zSp(H@B4KzLVdwF~=%5aJshE(9d{%v(ZbH5sH`t4;L8A^QGFCewm1x+`hbdQ)HO?FC zg6HoeDHs?|q=Os8SZj=xhGCN6%2{MyffWB541Zc2)K(pX?%a(ej9aRfdRC0JwQW}E zDten3Yx&#D$Q%5(NF4s}5+kLa5VRX78SUVFz?5j%{s>aM5B}5__DkSZqG7vkx(RuD zx(WGOw23_wBj*6QQB(#a3_5ZCnl7DJpwaWf%M~Ic?b@nQVs3XJWM z|CX$AcIUNd^c&MijYq8al9aKfY6tg2iuXa|3ZSK46_vq=s7~ik(joeg;Huci)jNP8 z(LT6NzyM#viKM-q*aw~CM{V3-53ZXldU+Rp_Hym|5B8!7df;ObJJ zRXxQL=X||$vf#?;;B_E?C5&Gv=-PffvZWiv2)>4sG3=VF)`F{cHYsh@VMrB`gz-(0 zmf9oMCEP$b86DJ8g#uA03|qw9Zj{ntp9UECl|2mfIJg?MJ<;5L4Y=YT#2Uk{9efE= zTwIJDN8jKwa8*b#a?b7F3q+k(?G=bRN3|jlP)-hFJ_;w2_C=85mjlKnPKW(xFeTA8 z(EudycV5QC(N@iZRH7AN3J~8*;Wl<>ZPlxgN;J2>2~+syIDs<4pmT6dy2>z4R0bpM z+NwW^%Fx6|`*KkkIA$4RrEl<3LA&#ds>dKTmmDkcIoMDnj`PLuY>_z5j`M(6my9sz zuul~^2wKC5yukzMahvU8;xD zmuOw;mhJ{C5THI3D-3|)M27uKB;GGc0yp51+Ag?i=9B7|Ze2PJUhx{vyG~@RIti`{ zd=AzZTp5duPUr4|t2~=kTB;`cU^kL5#!5@ILm%Xq1I{I`614XvtAJQIkvG_1aOG@X zr-Q4lWDTmI$Et^s!#w&s->UrzTyY_GT2)KHV2qW9p}l~ii!WBy07EfJINQWNJkMYE zJ724bSs9Yh0JK)CSb6He`*{s!2eUUk(^= zP+Rp1q&Q)k*kYvz$SSb{a9U>!yOw%ZAZiS|mby)F<^1Kq6{y0zh8t&5ZB+qc&TDub zU)bkEiq~)>4BD#y6WfF{?9<3s=gCkIXh07*d};5ioC%y(`^$y(s7k1xIzwvlhHwagEvBoCu8T25bA)dZe)$| zmzmnCj_5eNr8*ar>Oi!Ztiqq+WQ>)r46T4@IZ1HLBBcfiM4jWuSpre0dK~O25apOf zI=H=BvK?m*5amVJ*(TnH6jub}0I3n}BoOWD3;Xc`QDd9Xi2e~$ygM5m)Kaa}(S9eS z_^ZZ-zE~B4fJBSQLP+sq;*8bIblb!ZkvPUlMkD%xpq&#aV>)Z8H_~krtJ7^0FM;+e zi6>`exB!TbBq?VOE)%rp`Dni?-MUniE^!t?>h3hLdK4Wi(dIQvBQ&Ou1=5@$>t zT}8Jdhxx6Vb5wg1Imiv(_&Qo!wMOJ%6Ca}G@M^8v&kxBth9rF!1GKyr**~~)KXsvL~#S*L`pTr%)qZKjPI}X%rHP8YAiBZ zsvf-RMvfV8P)m&$TsarsqXbvHtwx4@FbKGh9CS)G68oStar$8&+(}kB5xp5)aVv#^ z;Y7x&emWSsqYrYpyZWrE39gDs!s*};0fX};`8fgxC+)kc{dc1`PRKe3KLrfDh8q#p zQfH=vp@)E>w-1I|0tVzzIFYo!3@P4moLsdRJC3t?-IH#|xfRWew+XHgk+GVG0PUr4 zJ6C`YA;l|zvE%6OyjvuWG3=W5&qU(zoRu+FT521*GcP8_SyW4%g&gcoVTZ5bL|U~0 zIao*%O>9y+>{UQ?GD#q8;bgQ*OSM3N5^WPtptkeEYYe-#YCaHsiX3xpULOhCoo(W5 zz|bSkD$UhQNbzD~jFp!9JES;aoWHWzC=hK*jz)4d3gv*e3FmzE9Hi!vHO7{zxjIkK zZcJw_^$n!%C990vgqC^|0eXO>j4)`an?U=+X{3r!Sq71mF$XoGKNIVckt;3L0JIMv ztBj>jOHIM}(U+unQjVnkSRl%4xKrwRLA$eF4NSM=6eHGGqzU_CNbw64QpCU9EB(c| zt1C)Jj+jzWI`HDMVV9PbPMlEw!=IfePn%Xcv3$a)YbTXz+8d^WVXANo+z z**>XZs0?dK0tSW?X;o8HhOH#ww5m*S z7>rY&=4vkL6|VqJu0DWNq7`5ZN?#$xsu_Jo!mwJvU@XX*t836wd7E&)NVx!9@xlvV z!-;fo8KmwZ3H<4i`X0|G+9vj<+a?x)%S7A67<_1gUu><(&H3h{Qo=hZC7NM<*YJ$uTFQU!k(_=H)ChLxAYMIIeWqUjU*Ub|dW?hVRlLx(kT5 zAqSmf?Yja7XV}*R2L7s%(>h}g>agE);(ymAr&X5&h8NPna46jk?oM}ux1bO5%K;}0 z+fc8#_w(uHm?1MT752vGjPdeHcrlb9HNb#$%65kCzA`+*aPpShDT}l#A4JQ)O9|=UA zv_B^hb$-=&9uQ5mE?odbd3QECsJrtrNY$Zm8)s3SINw8xU)ckE%MEI)_Tu@rB;k}Q z0`0sm;W9l|ErQf=lE8cvPDVtvgR@XN`;r9DSsA5t*iTKjE{#Tj){<4mma4643ECed zDI=mzJ`-ARyypibOI?c6$s4_M z^J)l08Oy4a??H-R4me@xD-d;7(M^y_v|6sjwedG`&I$QYI@vj zgF}!wjpD4*Vc!9%LXvW>mebN9`Ua%tlU4XLoX8vO3@&-$btcXUczz{W<18{SL5knW za0!fbuo@8MebAVL8qs}_;(gE}X4h2Y9*RRMZKF@?Q|J|l0i zE@0sAQk^$=Enrwj)?j=HC(^360tOi9ky?tdk0uEiXpzz|G!|j!@JBj000F8bYn)-P zhX73{Dfk*rq*bqqVcU3vI#xF$R=vp@XX1PUDc-!CW9`6nVSflxym>YAg?$~GUlmCh z`=B0*?ndJ9ELz~R>S#1So<*HEctCJvOlQs27&Q74atywPlaY2UH7wmMx+@Ld^my1|AUk((DShXZaBNOLdILO;X6Q9&^Ab__Cql22O zx5PGKl+s)+L9BX{WBjjXBV+Xiqe2Dwi0 zXU4GW8?1pGPBhkzhg1p0$k|f&LyE^*V-?j7R)|^DNV`s)F9o7b+6RkS6o`eB@dmY3 z+aWcTB%D(3qqff{DbN~Dq*a?m;y8cTcN$DtMby;7vJvB>Dec}5J|U3^x}MdHjM z3F8fFt3H6#%_M~z2qzNJ{>VX|+nwn=4bQJ5Yn(aQ105^Tu)SUk+fJ+6r=xwJ;Hs!Z0yux^R38CaMd5Zf`gb73D}Zrs*Ecv@(C*}_HlF7-+&Dn$ z#F+}I5{i|vifX9?km5DmxlJ6GZUq=4Rsf@e`UZ!kOPpOIah&PA04;eA-JlT$?ck@7 ZT0l|=TR4&RstoCTJ4rYb=gf5J{2x|w_iDT+0VvT>b>#y!-+Gf_tAcNhVFfhn(57zsvb<=S;qomJ{5UZMU*0ZTFEp zmK{E~q~dj?(`ZZ6ZP%gD`N|K(Y1f;sIDylKvgfzNsaB=a-IPwnfuQOI(%q^$fm;oq zWvBA62ismxSZ=$G`h9GJb<^`ZvgvzHwc>OFv2&%Byj{+%+68-KIWJ2arFA=J=f&PA zt>u@CmoJAZlE1!QSS_we^Fg`g)Z2}qTXp$@OajlBYNLZND~R?RwwQ4cwYs$=}MYz)Q>765p+MVNmh9ZRFu>ms{Sp8+dXlS1J|qrSgq+yRaggj_)^I zzua+yK7Qmh`Y-U@sJC3dz9SXi#U_0%yj6E(s~bq8DmQZlyKEP(<%=7(1dWz^Q|j;- z?nNo8{{$I1?JD$9-NN!}UIOQ#>(y!~!qq&B(e&zKgDci*=atl&sA!;b>vp8%Hl3ic z?W!%W(+C<~8yY<*PBSniD-|yl?RCk8@6f5V&_1N!+iNwqwk)fN12Xa^Bvf25A&(r4 z!2=J2(g|GDuu`Tuerj>iJkE;kDQ!dJx`&Dq(2h?uim-`(xoz3<8z}yY7pRkRT285& z@3oZgZGy%!g^ff;>CnY0IFk}oi(h=B~;SD zr+_B`(=1LxohAHT!$m^;B(d+5~+^N1MJ8E1R7bhql z%E`n)dTJ;S6NKS+m0@6_%a$B}hM_Jbr2k_+@m%wXXPXZfeh}~^Od=)Gtum1g6_~l1 z5GTh`u$S>_saLQj6=%-U<7SzH#^(D2GIdBh)Ei8tx$D6wL9YiVCi@*=qmv590V{71 z%Y=j31;Kf>^r%{CZjdrUj%9LJCygH-E;D;NpuWy);ME)!D?ASw;6#|yJdG6ZEXz!R zI-wawh6`5Xp1~&>8qXGv2j5d>Jwc7o;R0Y%pPiImk7mzd&=lCdC!T9Fc<}Vig;B#P z<5p#96z!JYM>o@G#1xaue2X0t>J313nkMQ^tVw;g!>kf9#S7Lr(}nCgpp~WG)&^&Y zbs2`G@9dJM%)Rn1XtS-Q-lK|1TL(OojpH!un8p%jLug29AxE0`i={6dYbL81hB21V z=_uax=+)p7($E;`=*E$b?kc|&y{?`E2Cr|t^TbAlUl6E##E1GAtNKL4PqBt`6bdg; zcu@;;5#et2cvMHP64oGK6U!zfhK^k5HR4kqYy^3_M&!<9r4D9(j+Z7G-$o;)k>UGRBc$NQA*$0qiAo z2-k14anZJ#X2sNJ4CX40%$%Bm(`h8M&{C_AhST)SMw?LAv?fav&;G{EO~OSG;dvhD z>7vgLjYVWyMp7KqdtniGOISa%A(i9VEBMTZ8(tr5Xd?K0K#KLX-c{BH$nTavH8wJzgh=Q*6u)8zVj#N6L Qt;;y9&An8lgt1ucKgmm=3jhEB diff --git a/script/teststig.bdb b/script/teststig.bdb deleted file mode 100644 index 4b9426d3872517d0d321201be6e7a14b09d6f652..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 28201 zcmb_lYmAlE6}<`-vBIEbY-Ma6K!+N@5n2j;fK_LpKA}8)AOjAg4u~Q%kal>O6q<-l zK_i-gR#1a)YsDJH2P74nYST*7NZNw0)Y3GfO{o@ti0Sg{S;-2&nzi@&bMK$Kve(+{ ztbM+7@4&4`)oMd(|66^_>zG>ewKpv4Xr4cBSx58K$6F2(*D`obC>q?G0I31+>wc`qm*|p2^x;7YH8&j&G5zMOo z0jN0zDrkU<#Q>DvFu;?58Bi)Zw!9iJ_2Q#be3;9nYe1})DfKoX#*JbSI|NHkFO}eJ zq@tZ5H?%;oV-NsUp0kcZFc9IX%CJ7Ks$YFV1`=@k`X=!?Cnslwn*Qr6e zHpGY7C7JZ)0%H<;HYbw~=Vj8%dE~YWxnq2}KNWIlV6{xSe=X!1V^Hhh?$!d)KL&Ld z2%c6T@U~he!Szzri7}|vIoWhK7!5C#zz@|j$<0Uic$VZKH=#yAr?ELzeV7+>mqH*8 zr$Ed#=EU3@!0=egs*D*oGEeG{@}#~@q#jm`V2~RuQgaI%FlUNJK&+PO?EmCxv;!J( zp9Kui3)_|lX2O`{ek=JsIHuDf=+ssqf|BnPo%jT9n4q`lL>N=24ItNA$OVt__vYny zXI_4Tqw`E3$A3Ngo1Tu|-~3EqG$U&#sH z{;=e{9F6u!@bXc|l=~iZ?)#-G@T`^z^{VK^+d;!ibs*Q7Q_P0pzB$`DzlIAR5u(Ce$$~g)tYDdjX&b z!B<14(b6>@b_|%kl1*QYZInKuQBdxMqETSZy#^TGpRgiBqk(x5w=Ylb-5|$(#)vb> z)eAYon49E{JaStlzk|n$Z3vW+#o2gUEt6mm2r{?%YC^p!TAdNAYLf&nAI1zLOwJ45 zrvUZ!LJBUemZ{V7Jju_?8wX=^!gq>D9^88?@^os+)9HA0pJS5&Y#L!Ppg1lAE*~eI z<3p>lSytxB-I^zNzdX5b6uEf;V(9cbU=oJZAjr*!3?6n&AFTq^$t9D5du}2+H!cV2 z5kT>|F8D*F{*ZibsS2poGPT+x-3wfWHc0ol=gf-!3aFulRB&w8f!w?Tg&eMyS=DA3 ziC=~Uvgz5pvH5PEP6MFRf}&6GD7FPUeWgGFwOU4~%#g9kkzjUiyGR~PgTF!Y(+a7e zpT?sUo-awz&}pXV#HT1zr~M!|qL5=nCe$)?o)En2HONg6jY!UbnFWm)xU{56Y&A6E zWC?DR3lZF$EWyNl0#KYR)X5~c7XopZa|#=hH$o>4^WdTI7ebDendJ5fIZAGjs|O6z zlD;x4+M1J=J0#-RHM61zq@uuiV5d|>sm+Ss%gd$Z=v+cBohP|OOBz0!otH~DOD^#? z%#{0p$Q=xmDH3tPv|N$Xxhq9-G_hKyPNzesg#`kr5vZpn;)1^yM7$SkML{HyVf93BxpiCnv;~fBq@Vi_j@_H^q3F}bnT~TKF>-) zV*dgZkEX#5GD9+nqrf!4`64%~GB(Lol1ahgy-p+#y8EC=&dVWFrnMkp46*FYM{D9>4m(C8f6S{>zY1#1)cj= zA%(ZqGNE3~Blx6bIQ?YU@>MW8w~&Hl5pp{~Zfb!*aS^CXa)Ni8XvE{Gq0#LUy1}G8 zB%#YcRW!YH1?1-S0&~fPdM_v9CPQwX2LhJ7R+2JE>?}!2P7}ib8^i#-?J}jFo!7OS zC3FL)xC4M;LNeYAjYdHuR>4D#0dpnv`DO_Zyp4p&=^)03ir}_gD^dr@y$=}fdc3Wc zq0t-&lQfW;@-%uaZ<4fuU`x@5I+^?JB|vc~aRiu9!$d2_oC)=e45q-}73>ztfmkiG zqEBQn1tV^5UiYTwOq4~?iPs8@I8&#MG79+ZjtO;GBp(#(-aL^!XoMLeIq$xWjJh5= z9~pI~PT!Ep5;#_@!}g^K0vUb~Fh>$I^jQxW?tDJW88H8rYS?%K<~Nd(Y`WR45vYh$ zH|XjYz=+>8Q!9hq>ynaeyh-jwsfb5YqoO@h5glz-bWdI$9Ssxk4vzn*i&@pxl1IU* zd%5Hhuk#GayYlkr+mc5lXH>K~uX9(3MuCmeEgAtaa-7(bM{a{8C2yGxxj&YqWG)#n z<3WrzWb@5Uaxb7*3HD1w?%zp%bHequS^yWD6@NKOZS3jg8KnAwCKalV^-A%-Q$o7+H)bGT1r(xf-j+arxqyM z(+HPig|B7@5Aj(ZnuABMjo5RXae;Ag7%;4eIvM)B41I>h z8{Z@P1P_FpL?6bRLF`k}hrw&W{1J}k^$RZ<&7D6M74gAhaFN`EdK+?2I-+YPREwk} z?Px+hl$Vl=CE9}Lfp?(Qf!xUABcMk7v{|B#cX0-}UeJi6AP{xe0EQpuj5Dek0DZ<3U4onB zPk>_h^4l-7Yu}OZ9TMx>7U>$V(G8t?LvCJF^Lr-)=KQ?;{&Sw(8oXK;|ryO!9;jMIv9l73)!a?Vt5-Ncy^xb zYal!GJUFQKfz|XvEC}^o5L}o8bzRQD`Y{*{DHWl(YMJ(|hfYbe?+rllzdHz4bq+qy zfzDTB(rg6`A6)4x1Ez1z+*>5g=M$$<%}LTRk~5qCW?sjR6O95tkX(+6T8g8C5%-pm zLvfK!asc~{4`j5XDftu`2fPF^p?)djAoydO6?wA1FIv&%Cc%|?Ro#-8n5*)tnwS%F zkL1MM63HiC85{ZZ6OlT&?;0RAhf~nuHvy`(q(HE$Nzyes+|cRKoUYY@4Ijz^Gp-9T z{5YS14N|8_$5@rQ``-l=j}w&HgnA58^KyvIH^|M%%kJ%x-9eoWO5l>Dsnbj$NHdsF S&k4ccoSOwGS`u6%gZ#hZyKBS% diff --git a/script/teststig.bo b/script/teststig.bo deleted file mode 100644 index d6597dd548eaa378639de69f9724ca89cea8f6e8..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1478 zcmaJ=+j7!S6y1m^P%mx01xS=#q1O9@kIwj@KhPQ52j4J|GXaGJa{`rqjN^>IqMzZb zZ~lX8pA#s7)`u)s)?WK^b`CFT9ApEUss1a9)hT_Yx%^J3pJ>vxKG)@-5ZmfpU0BwN1B-PYQlOXH| zI-_Ac%m|H7161#)3bp7{f-z>Kvtcw=X*8#dCR02#g{CGnN+LRz1lGed^_uDZ(YvFQ zV>%fqo=FxD0?jzgnIcvrHCAz^DN^VbQZ}AYsx)3grL#1d6W<}C{kK<%>g)>rXy`+s ze+_*u^rs_+7eaptB?cvzOTb2R<;CMGklJs6MHyMt{;(leMgFlNZ;SkAL%uHZZh5`- z4UxMW@=cMyZOA(!|K5;yO>G>x$$OxD3sC3Vz!n~=ml|b85_ROv%`(GD9ZX2gEoU{v zkg%C8pl9MON$_in6nSGj@Kk_B&z6&iY9?g7($q6uO@FUux%AIvLu(b=Xj$Ugnqj3x zOXl{MPWcG7lr3=};li2h3?gqA&S8hJ`q~=KQ|#E4OA^f@y~_F)*VC*2O|M>_UR?b7 zz%`_-ti*h(3l=PAvm@3jSlC^>x_l4lnQP*j8{*o9I$Rfb!+4tFG5fSjt(&0Bp3wp1 zvk&C^0GF*@{v`|h1Mo)OcwCk{=rIFX^t@pqxDVa-lbL;o+hXJnkPn2~A#?eW$kN~C zL!f6axzCTmTe5pU_gTvmvGB5kZ)fDEkX^&~4!>h|eg@&9nMGgpw-xPyOhp;DFxzTR MmS72x_jDZRAAqlwSO5S3 From 013bac1c5b6eb5e77ec1c810240d6b7218e07956 Mon Sep 17 00:00:00 2001 From: isvogor Date: Tue, 9 May 2017 15:48:38 -0400 Subject: [PATCH 13/26] merged.. --- launch/rosbuzz-solo.launch | 6 +- launch/rosbuzz-testing-sitl.launch | 12 +- script/testalone.bdb | Bin 0 -> 6375 bytes script/testalone.bo | Bin 0 -> 508 bytes script/testalone.bzz | 6 +- script/testflockfev.bdb | Bin 0 -> 64884 bytes script/testflockfev.bo | Bin 0 -> 3676 bytes script/testsolo.basm | 862 +++++++++++++++++++++++++++++ script/testsolo.bdb | Bin 0 -> 49136 bytes script/testsolo.bdbg | Bin 0 -> 49136 bytes script/testsolo.bo | Bin 0 -> 2969 bytes script/testsolo.bzz | 215 +++++++ src/roscontroller.cpp | 3 +- 13 files changed, 1092 insertions(+), 12 deletions(-) create mode 100644 script/testalone.bdb create mode 100644 script/testalone.bo create mode 100644 script/testflockfev.bdb create mode 100644 script/testflockfev.bo create mode 100644 script/testsolo.basm create mode 100644 script/testsolo.bdb create mode 100644 script/testsolo.bdbg create mode 100644 script/testsolo.bo create mode 100644 script/testsolo.bzz diff --git a/launch/rosbuzz-solo.launch b/launch/rosbuzz-solo.launch index 26ba3e7..97cfb46 100644 --- a/launch/rosbuzz-solo.launch +++ b/launch/rosbuzz-solo.launch @@ -30,13 +30,15 @@ - + - + + + diff --git a/launch/rosbuzz-testing-sitl.launch b/launch/rosbuzz-testing-sitl.launch index e086595..6d3da79 100644 --- a/launch/rosbuzz-testing-sitl.launch +++ b/launch/rosbuzz-testing-sitl.launch @@ -27,15 +27,15 @@ - + - - - - + + + + - + (#+|BkDSq%9PmIMeKT8w<2_db0vo(Jt{V&z-mDfyVb9 z)cE#DZ&y6$q>Amh0S=zu3}!ZvT#)bfC^{<5=$}Gs6*K+ys)F!tl}Ncm+9MsOn2Gd< zV@JDN=~;i?UP#K!@4?LW5+V1@SY{4X%mhq0v9@JOE&AWUyoP$+D<1gK?osvfZ_9gB zy$ZxfJC{zvRmZm(yt!g?{t9g2OgL1*v9?W7;C!*=o5on%Lk^DhRy^_d&K96iX_D>s zTAYUh@EwJpC$B3G*OE-zeLEh7<~s`i-riWLDnD8!#bNG{v0OUr;8G!OLrw>OCw>96 zBB_&~OOJt>N2^79_0Lbr%;`W&r?XlsEtBWw`6`E&84VtChNAhxvFH2;2Qzo!CZz}T z$nHsz;(4Jvu74|@`xV6F;CX-X$scJgKyU)raYs4>Xz6%<5NLd#;MZ{@Fwb{Mgn8~r zSF5f8ZNrJ3z+=!gd?)6=3zp*Xd;^emuQ&qR4&r^*D0Zuk;~ix0xP|DE2I2`fEIZmv zFlU0GA=%;B(f*})*wc;H70J3mQ1aYp4+3p=GPG(Kq11r>nob59-%e1o+p92A^{;Wlw7qCNc5+s&^ix!a zS}ALF`XCXFBnrRY*$)oiCup3I_6%1-LZ~VDI|n;jGo>l`{kR>i3hG5&%v&jz)qhj9 z<~P@|wk=hyv+|@bYTaC_bBui2Gtf5aUVSoZ{gC(s;Bf4|y@m{PXJDkU87@`ZY?0fO zZ<21a{ZPTn#V1^|;n=hE6wuP`#`$n)i1_5MU^_tEFKBQx9DAhe)&9x6#qwc-;*x)E zybd(}VhMkS6YI(GxJf&R4b@w$?Tc#vtR@)teBP$^Pkys!NBbYuBQ@N2%y$#Rmi(jy>lsm5%f89@S96PU2HmrSLP9JYQ@%S7WRH zS<>pir|N}aAC5i4Rna5qv{)-uuexsEs-s>%7Zgr|*qk@T&BgEInz%g~gh6?^pdkI> z*mHgX_(1hUJ!1E52GG)N`;$QT-I9IY$ade}09v~J{B1bITC&4pyK=Q- z8jMlXfIN?V?qAe2sPAU=QZ)^j343*}RIK)YTl56bW=n)6?nrB^9`V1YovC`nf3rNO z6cG7fufk;2aUf|pcC>v-JFYz(q9!6kVwwMBQa-$o3f?E#;f`R>(woTA6N2V{L((Rx zCaQrcXVpX<9QCR|BDzXWu*8L9&+u1hv;PtlB-@F$RdK1I8?Ad%1#~9DqrRqgvFGhF z#iaswq%{?nI9p=-x1-`xo_G>#TLrgX2eDyJVr^UEj-#&kE3aW-bvN+@Bnrn~ogRug z{#)rRFy~?M3FFP_+oNb&?(|vTefk=?wlNl>K6-&f6ng# z1ixYT-}|LX?=bgvX5N}q?`#2=_;wG}2q(6J|9}B^Q>vR-+x3_^&EeB9cf2doc;f$dzCT7PbusrtyKe<30d;Y8cNOLW zjk=h8u>1C~+WGxen2O22TO#zo58UABnu(6C`Mc_x2JU(-S6x%jjdrzSW-T|`C*ab3 zg5same*~8v6g0GBICf^f11?d^_<8<-;u8K0Cw7WXa&YN8BqE(&cm~y(v#(wfY&i%0-`ZCPmpX_C)iQe02Db*) zqF*nZ1ZEbC2lz7_d-a=wndO4QY}wIzD`xt4hVv9N0ql&lvtlOmXODF2ad!1F3(TbE z^iTc|fW|4ntuMAmE>p~G@6K=~#Z1n5dxrlEG%8ufWk+k4l;?K>jo;kWcIT}SBuJ+z z<^zqI!k@S4Nmal$)ivBwV!8UE;yJq~*7mKWDqvMo74Rx}ezkDOKMgJhGlvSAzXz8n zp69uFJ|wB$DN5Qk3xIY{vb;Tx>nfdknxT5o|E}gB(5S`ONA|h;5Zah;!?JE@Z=)-D zuk&rN-ILX-E9<+tT8?0*3yR;j-9VcoX#O5A2E<}PJJucPe}J|^P&kwARd_-%-yi7} za427V<7e#5p9*$S{bgn50od}FOx*VEIk^@SzMDkh*OEB|!?zHAEjbiuGsQP%$sXx* zfFRG=*>B>`qbBCW~}YGj#_t8weI4sbyZbsG)Fjg4!;I8 zY8F3pTA?IE#V7wAbWc*vax3m2>H^LHd*0?CS-T}t|0e$t(74I-X0iLWL-h!&5XyOj!p8)K~PdFZc>l25oFPOR;E^jG2k+x+;4J$uW*x`~2@WQ1c!Ym7uJ z78L)6za5jAYLpqa`!*M>dro}wE2EDUhy8Q@eDI;uQNG3UVLEK77nr43+ke8Ay!ZcN zVS{331Bu37ui*B{!(cfvMbiM7I@xMp35m0YdZ-$LpQPI4r|Zn3E&}BupjMZ z#cKZ)>XlS|6eH37jX7TkTfV;#h@3s=wH?);tg2rmuGl>}QB~jno34TA^tIv>+ziL= zTLX0Z!-C@XtxR#rzqc<^eDKe-JjDkLm2hIGK{woS?AxwlYtMOgl&x4$$ag#1n?R$w z@qdfmMyVTr&hG$3Iz95d+WDEc*b06HE>RcrGuC#i+WFBg;l$dWscP-twe|yTfOz2F zjxIy3hY1>h!?E*lWl~LXXHw>TfwDcBEYfX?58T{h`7j$(hQe+Aj@`EaM9c@=q z6|evVO{W6-ffo4^=>_r`+ck4k*Dy=5wjZgk@xR4-NOet9iNap5G2r=)g5qc9Uw}48 z(EPhxbJaCy?r`jp9s$})LGj-;yA{v<_uxXsb3a;RY#LSt)Zft~A1j{Mch_qwiquU| ziri?Yf|=C6er66uk=7>TQVFV01ysi!=~YP;(1nUiphh^cb8j(hxg{gSaP0kiwS$@U zz)Ze};f)y^MNf=HDz9GdD5fb^7rW6agVomwivQQh6^hmVdD{m(d`5f&5yG)o|79qz z0zt|82JbsOo{Ec~vDF`-SjSy8*0z^oPMJFcHNhM*#=k|p3$%1Pt|RC&Od|C2@M{bU zY9;^N=nEdccocK&Jp2=Q$SRQ4v3&U6!NX_4Lw@U*b^m-%)!LuWb*MGJxnoD#^LY=( z@iM87-?x6K^()D0eaKO-9gce4g4>D;J!`GEVCHa6`PIP$+%GpuK5;pOW3Tn=K%+Xy zbE91X=1^0ElHu5WJ09V!6cn_J6YWKyZ4?y$w|=*wF{pxhf7v5F>|pgy2dn2hSp5ZT z`QE9-U7g2NkNE#LlGfni?cx)F!?9QACyIxDR==cp=>MMXLhvx1PPrI7q)KI^u{Y@w z*jATZa~rhh{0E@%%{T}iPOK-pV9-Krvf37b)l?-YRHo-HfbAf$!AcTNtj{wr;JS$o zcNu&3&Pb|3Mj#Pu#W&u4cHf$Sv5yEEIc7&|t{97;!->uLQrNZ?8!Rc|*gYu+)$#?U zP(EWlSqSC~78~|XtnHN;aMZ$nMN=C*B>#%UBYRF}CS`S3&}EqTfqrb@Nk4=7*}XGu{^vgDGzT1bGnNM>;q=<{j#4+v%sY;;)%cdA1G#Wd$RL< zlj2f6@zIWU9VX;5k(rH~CoGZ&Z&o zl1$h$@RVY8D>vHpNW?5bfmjL0?%NpDdaa=Nx1i=2M(K3iB?ymi9{Agy*a|L#Ep?o~ zBN{6%W%Z;0p2TJE-!d@sSAyc#p^qw_`}dajQ1ZFr8>?@Rv=h1}-Nw64^&oEt zyKhwiu~H)SPx#k>#{HSv$nM)o=p*jW^ewiJMyVdmx&yxgTkhUSb~v%mJ%o#m8ndz3 z+A}anO#?<@N86#c@t?VU>y3^ZB`E&e{J+s=vjok*+y6uLufIn+C+*+U;Ls|G6e$eH zUen8vtigi9DH_Ywl}ahFzwDkg0||PHC;UxitS5(6*ZALl>{L8&BOck~ny#3M!4Qre z?Q(GG_k!ZTStf!@>jcgJ9Z+kfV*DFaTd-@m_{NTo<-;l%^buQbQ1+ZJ0lP*B8mDNi zCq>wKipADH&ugLN(*(`WrAt-4{QbLBWhtxDFIHJqa^jwk492toP$y*m5J~JdE|ZLhX=bteyW~Dc1Qp+(+$@piDS+-?jp6vY`0U zKESM=D`+5KICkGQsg5|yjdms+S|up{dw)OBHVGQa8;(8FDym1ww^*)@Q$51D5o`N5 z)g#yu zs~xV`MZ`aUdv!RLY~J-68qf7xCV;FwuyY&Gf<#v?eFxd zsP%gB3BcjleOsfnhW{JtQNrnb};9{q}5*xv@Q}wzI@v2Ras@& zzkkj^hS8qk@H4hY4#Kvn(e_eQr(-+JL0{{4+`S z`A5_Se!e@>nn>0vLGjiq;{lugj_(-*ZGYWex1}5!@XH`e|yLBF}&skD0*4iHFXF!`LXh>l=cC<2dEBASS KN1U6qTmKib#r{|T literal 0 HcmV?d00001 diff --git a/script/testflockfev.bo b/script/testflockfev.bo new file mode 100644 index 0000000000000000000000000000000000000000..3cb83de1bcd58d79ddabd6bd34d7d4115e4f8bfe GIT binary patch literal 3676 zcmai0TXz#x82u=QUW8(xI-!Z=)RYDk73vM~0-+5hQW{LBT*a(uG6{hsQzw%`d2spS zgAcBS?>_iDT+0VvT>b>#y!-+Gf_tAcNhVFfhn(57zsvb<=S;qomJ{5UZMU*0ZTFEp zmK{E~q~dj?(`ZZ6ZP%gD`N|K(Y1f;sIDylKvgfzNsaB=a-IPwnfuQOI(%q^$fm;oq zWvBA62ismxSZ=$G`h9GJb<^`ZvgvzHwc>OFv2&%Byj{+%+68-KIWJ2arFA=J=f&PA zt>u@CmoJAZlE1!QSS_we^Fg`g)Z2}qTXp$@OajlBYNLZND~R?RwwQ4cwYs$=}MYz)Q>765p+MVNmh9ZRFu>ms{Sp8+dXlS1J|qrSgq+yRaggj_)^I zzua+yK7Qmh`Y-U@sJC3dz9SXi#U_0%yj6E(s~bq8DmQZlyKEP(<%=7(1dWz^Q|j;- z?nNo8{{$I1?JD$9-NN!}UIOQ#>(y!~!qq&B(e&zKgDci*=atl&sA!;b>vp8%Hl3ic z?W!%W(+C<~8yY<*PBSniD-|yl?RCk8@6f5V&_1N!+iNwqwk)fN12Xa^Bvf25A&(r4 z!2=J2(g|GDuu`Tuerj>iJkE;kDQ!dJx`&Dq(2h?uim-`(xoz3<8z}yY7pRkRT285& z@3oZgZGy%!g^ff;>CnY0IFk}oi(h=B~;SD zr+_B`(=1LxohAHT!$m^;B(d+5~+^N1MJ8E1R7bhql z%E`n)dTJ;S6NKS+m0@6_%a$B}hM_Jbr2k_+@m%wXXPXZfeh}~^Od=)Gtum1g6_~l1 z5GTh`u$S>_saLQj6=%-U<7SzH#^(D2GIdBh)Ei8tx$D6wL9YiVCi@*=qmv590V{71 z%Y=j31;Kf>^r%{CZjdrUj%9LJCygH-E;D;NpuWy);ME)!D?ASw;6#|yJdG6ZEXz!R zI-wawh6`5Xp1~&>8qXGv2j5d>Jwc7o;R0Y%pPiImk7mzd&=lCdC!T9Fc<}Vig;B#P z<5p#96z!JYM>o@G#1xaue2X0t>J313nkMQ^tVw;g!>kf9#S7Lr(}nCgpp~WG)&^&Y zbs2`G@9dJM%)Rn1XtS-Q-lK|1TL(OojpH!un8p%jLug29AxE0`i={6dYbL81hB21V z=_uax=+)p7($E;`=*E$b?kc|&y{?`E2Cr|t^TbAlUl6E##E1GAtNKL4PqBt`6bdg; zcu@;;5#et2cvMHP64oGK6U!zfhK^k5HR4kqYy^3_M&!<9r4D9(j+Z7G-$o;)k>UGRBc$NQA*$0qiAo z2-k14anZJ#X2sNJ4CX40%$%Bm(`h8M&{C_AhST)SMw?LAv?fav&;G{EO~OSG;dvhD z>7vgLjYVWyMp7KqdtniGOISa%A(i9VEBMTZ8(tr5Xd?K0K#KLX-c{BH$nTavH8wJzgh=Q*6u)8zVj#N6L Qt;;y9&An8lgt1ucKgmm=3jhEB literal 0 HcmV?d00001 diff --git a/script/testsolo.basm b/script/testsolo.basm new file mode 100644 index 0000000..3ec341b --- /dev/null +++ b/script/testsolo.basm @@ -0,0 +1,862 @@ +!83 +'updated +'update_ack +'update_no +'updated_neigh +'neighbors +'broadcast +'TARGET_ALTITUDE +'CURSTATE +'TURNEDOFF +'TARGET +'EPSILON +'lj_magnitude +'lj_vector +'math +'vec2 +'newp +'distance +'azimuth +'lj_sum +'add +'hexagon +'statef +'HEXAGON +'map +'reduce +'new +'count +'scale +'log +'Time: +'timeW +'WAIT_TIMEOUT +'land +'BARRIER_VSTIG +'barrier_set +'barrier_wait +'barrier +'stigmergy +'create +'barrier_ready +'put +'id +'get +'BARRIERWAIT +'size +'idle +'IDLE +'takeoff +'TAKEOFF +'TakeOff: +'flight +'status +'Relative position: +'position +'altitude +'ROBOTS +'Altitude: +'cmd +'uav_takeoff +'LAND +'Land: +'uav_land +'init +'s +'swarm +'join +'step +'rc_cmd +'cmd 22 +'cmd 21 +'To land +'uav_goto +'uav_arm +'uav_disarm +'listen +'print +'Got ( +', +') from robot # +'Current state: +'Swarm size: +'reset +'destroy + + pushs 3 + pushcn @__label_1 + gstore + pushs 11 + pushcn @__label_2 + gstore + pushs 12 + pushcn @__label_3 + gstore + pushs 18 + pushcn @__label_4 + gstore + pushs 20 + pushcn @__label_5 + gstore + pushs 34 + pushcn @__label_12 + gstore + pushs 39 + pushcn @__label_14 + gstore + pushs 35 + pushcn @__label_15 + gstore + pushs 45 + pushcn @__label_20 + gstore + pushs 47 + pushcn @__label_21 + gstore + pushs 32 + pushcn @__label_24 + gstore + pushs 62 + pushcn @__label_27 + gstore + pushs 66 + pushcn @__label_28 + gstore + pushs 81 + pushcn @__label_48 + gstore + pushs 82 + pushcn @__label_49 + gstore + nop + +@__label_0 + pushs 0 |8,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 1 |8,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |8,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 2 |9,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |9,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |9,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 6 |14,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 3.0 |14,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |14,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 7 |15,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 8 |15,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |15,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 9 |18,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 10.0 |18,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |19,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 10 |19,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 18.0 |19,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |20,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 33 |72,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |72,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |72,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 31 |95,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 300 |95,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |95,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 30 |96,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |96,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |96,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__exitpoint + done + +@__label_1 + pushs 4 |11,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |11,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 5 |11,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |11,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 0 |11,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |11,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 2 |11,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |11,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |11,39,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |11,39,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |12,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_2 + lload 3 |23,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |23,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + div |23,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + unm |23,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |23,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |23,44,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + div |23,44,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 4 |23,46,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pow |23,48,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |23,58,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |23,64,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + div |23,64,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |23,66,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pow |23,67,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + sub |23,67,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + mul |23,68,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret1 |23,68,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |24,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_3 + pushs 13 |28,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |28,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 14 |28,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |28,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 15 |28,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |28,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 11 |28,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |28,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |28,41,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 16 |28,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |28,50,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 9 |28,58,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |28,58,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 10 |28,67,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |28,67,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 3 |28,68,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |28,68,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |28,74,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 17 |28,75,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |28,82,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |28,83,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |28,83,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret1 |28,83,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |29,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_4 + pushs 13 |33,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |33,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 14 |33,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |33,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 19 |33,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |33,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |33,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 3 |33,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |33,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |33,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret1 |33,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |34,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_5 + pushs 21 |38,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 20 |38,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |38,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |38,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 7 |39,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 22 |39,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |39,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 4 |41,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |41,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 23 |41,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |41,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 12 |41,37,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |41,37,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |41,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |41,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |41,39,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |41,45,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 18 |41,52,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |41,52,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 13 |41,58,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |41,58,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 14 |41,59,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |41,63,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 25 |41,64,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |41,67,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 0.0 |41,68,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 0.0 |41,73,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |41,77,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |41,77,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |41,78,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |41,78,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lstore 1 |41,78,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 4 |42,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |42,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 26 |42,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |42,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |42,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |42,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |42,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gt |42,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_6 |42,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 13 |43,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |43,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 14 |43,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |43,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 27 |43,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |43,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |43,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 1.0 |43,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 4 |43,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |43,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 26 |43,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |43,48,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |43,50,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |43,50,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + div |43,50,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |43,51,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |43,51,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_6 |48,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 28 |48,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |48,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 29 |48,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 30 |48,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |48,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |48,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |48,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 30 |49,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |49,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 31 |49,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |49,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gte |49,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_8 |49,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 30 |50,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |50,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |50,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |51,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 32 |51,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |51,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |51,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_9 |52,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_8 |52,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 30 |53,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |53,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 31 |53,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |53,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |53,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + div |53,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gte |53,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_10 |53,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_11 |55,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_10 |55,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_11 |58,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 30 |59,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 30 |59,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |59,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |59,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + add |59,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |59,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_9 |60,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |61,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_12 + pushs 21 |79,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushl @__label_13 |79,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |81,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 36 |82,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 37 |82,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |82,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 38 |82,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |82,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 33 |82,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |82,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |82,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |82,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |82,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |83,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_13 + pushs 35 |80,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |80,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |80,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |80,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |80,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |80,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |81,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_14 + pushs 36 |89,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |89,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 40 |89,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |89,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |89,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |89,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |89,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |89,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |89,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |90,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_15 + pushs 36 |98,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |98,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 42 |98,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |98,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |98,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |98,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |98,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |98,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 7 |99,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 43 |99,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |99,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 36 |100,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |100,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 44 |100,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |100,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |100,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |100,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |100,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gte |100,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_16 |100,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 36 |101,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushnil |101,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |101,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |102,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |102,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |102,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_17 |103,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_16 |103,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 30 |103,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |103,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 31 |103,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |103,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gte |103,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_18 |103,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 36 |104,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushnil |104,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |104,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |105,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 32 |105,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |105,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |105,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 30 |106,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |106,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |106,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_18 |108,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_17 |108,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 30 |108,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 30 |108,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |108,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |108,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + add |108,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |108,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |109,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_20 + pushs 21 |114,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 45 |114,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |114,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |114,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 7 |115,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 46 |115,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |115,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |117,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_21 + pushs 7 |120,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 48 |120,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |120,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |121,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 47 |121,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |121,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |121,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 28 |122,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |122,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 49 |122,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |122,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |122,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 51 |122,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |122,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |122,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |122,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 28 |123,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |123,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 52 |123,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 53 |123,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |123,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 54 |123,37,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |123,45,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |123,46,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |123,46,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |125,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |125,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 51 |125,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |125,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |125,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |125,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 53 |125,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |125,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 54 |125,37,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |125,46,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 6 |125,64,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |125,64,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 6 |125,80,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |125,80,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 20.0 |125,81,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + div |125,85,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + sub |125,85,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gte |125,85,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + and |125,85,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_22 |125,87,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |126,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |126,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 55 |126,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |126,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 20 |126,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |126,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |126,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |126,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |128,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |128,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |128,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |128,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_23 |131,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_22 |131,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 28 |132,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |132,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 56 |132,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 6 |132,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |132,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |132,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |132,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 4 |133,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |133,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 5 |133,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |133,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 57 |133,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 22 |133,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |133,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |133,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 58 |134,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |134,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 6 |134,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |134,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |134,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |134,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_23 |135,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |136,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_24 + pushs 7 |138,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 59 |138,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |138,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |139,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 32 |139,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |139,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |139,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 28 |140,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |140,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 60 |140,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |140,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |140,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 51 |140,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |140,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |140,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |140,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |141,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |141,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 51 |141,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |141,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |141,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |141,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |141,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |141,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 51 |141,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |141,40,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 3 |141,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |141,44,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + or |141,44,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_25 |141,45,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 4 |142,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |142,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 5 |142,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |142,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 57 |142,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 21 |142,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |142,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |142,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 61 |143,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |143,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |143,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |143,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_26 |145,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_25 |145,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 30 |146,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |146,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |146,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 36 |147,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushnil |147,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |147,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |148,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 45 |148,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |148,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |148,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_26 |149,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |150,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_27 + pushs 63 |154,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 64 |154,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |154,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 38 |154,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |154,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |154,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |154,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |154,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |154,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 63 |156,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |156,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 65 |156,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |156,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |156,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |156,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |157,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 45 |157,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |157,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |157,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 7 |158,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 46 |158,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |158,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |159,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_28 + pushs 50 |163,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |163,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 67 |163,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |163,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 22 |163,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |163,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_29 |163,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 28 |164,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |164,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 68 |164,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |164,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |164,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |165,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |165,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 67 |165,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |165,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tput |165,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |166,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 47 |166,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |166,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |166,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 7 |167,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 48 |167,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |167,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 4 |168,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |168,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 5 |168,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |168,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 57 |168,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 22 |168,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |168,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |168,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_30 |169,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_29 |169,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |169,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |169,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 67 |169,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |169,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 21 |169,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |169,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_31 |169,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 28 |170,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |170,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 69 |170,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |170,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |170,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 28 |171,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |171,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 70 |171,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |171,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |171,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |172,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |172,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 67 |172,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |172,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tput |172,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |173,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 32 |173,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |173,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |173,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 7 |174,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 59 |174,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |174,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 4 |175,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |175,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 5 |175,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |175,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 57 |175,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 21 |175,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |175,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |175,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_32 |176,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_31 |176,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |176,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |176,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 67 |176,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |176,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 16 |176,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |176,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_33 |176,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |177,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |177,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 67 |177,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |177,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tput |177,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |178,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 45 |178,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |178,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |178,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 71 |179,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |179,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |179,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |179,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_34 |180,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_33 |180,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |180,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |180,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 67 |180,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |180,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 400 |180,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |180,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_35 |180,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |181,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |181,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 67 |181,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |181,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tput |181,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 72 |182,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |182,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |182,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |182,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 4 |183,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |183,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 5 |183,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |183,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 57 |183,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 400 |183,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |183,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |183,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_36 |184,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_35 |184,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |184,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |184,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 67 |184,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |184,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 401 |184,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |184,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_37 |184,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |185,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |185,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 67 |185,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |185,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tput |185,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 73 |186,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |186,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |186,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |186,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 4 |187,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |187,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 5 |187,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |187,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 57 |187,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 401 |187,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |187,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |187,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_37 |189,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_36 |189,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_34 |189,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_32 |189,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_30 |189,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 4 |189,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |189,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 74 |189,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |189,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 57 |189,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushl @__label_39 |190,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |203,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |203,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |204,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |204,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |204,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |204,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 28 |205,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |205,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |205,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 7 |205,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |205,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |205,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |205,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 28 |206,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |206,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 80 |206,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 55 |206,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |206,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |206,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |206,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |207,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_39 + pushs 75 |191,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |191,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 76 |191,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |191,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 77 |191,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |191,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 78 |191,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 3 |191,59,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 6 |191,60,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |191,60,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |192,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 22 |192,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |192,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 7 |192,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |192,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 46 |192,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |192,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + and |192,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_40 |192,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |193,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 47 |193,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |193,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |193,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_41 |194,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_40 |194,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |194,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 21 |194,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |194,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_42 |194,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |195,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 32 |195,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |195,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |195,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_43 |196,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_42 |196,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |196,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 400 |196,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |196,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 7 |196,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |196,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 46 |196,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |196,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + and |196,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_44 |196,44,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 72 |197,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |197,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |197,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |197,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_45 |198,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_44 |198,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |198,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 401 |198,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |198,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 7 |198,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |198,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 46 |198,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |198,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + and |198,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_46 |198,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 73 |199,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |199,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |199,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |199,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_46 |201,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_45 |201,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_43 |201,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_41 |201,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |201,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_48 + ret0 |211,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_49 + ret0 |215,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz diff --git a/script/testsolo.bdb b/script/testsolo.bdb new file mode 100644 index 0000000000000000000000000000000000000000..877ba46ee655f8ac2f8f2be266af4c87c3e72cdf GIT binary patch literal 49136 zcmb_lX>^w55&aZVKn1j*5=1^F34Zw^0bEcPMH3ZJl%;?u6_b#VU`RAy5QvB691@O# z%Ax|b;6jicTtFdugeZ$kP_Zr0R%`{UP%GAoRzT6-^LD0Zem{4fKjcsDnYnXk=DyGS z=1|il7W-A~f9GuZ_lj6fNyYf0oYDvK%X13ztL`l=A3ZUZld3GtsjNs9OsKBTNflO> z-dB}VRg|hqRg_h9DX6YK45ae~p(lm(;%Tv1LJ;~=NM&clVl{%0P9ZI77K_yh!kHA( z_U5tJ20=KBLi(mfEY=_h0F6#q?bsCVhZUb7+)y}aSCY%$k`|e z0F6!@J!T4cQ-=>`qUE@_n6p2}{7QD!!UGzdFb=l8i3(n_dcf*_niA)RGZz-{wC z0_hQnr2`ey7^vU{K>+#D2~)vbs9={MAUp!;VK}iig1|9;#CW>Cf*6+FV529lTV4TD zLSo6KVp;`dP7(xe7Tk}5)d&)&%soJQ)Fq}Rm~?f5#7*CMMwyU%bi!2dJ&+m%f$Nr8 zP-Z_tYAOGQ#q_GtQyzs>iQ3h=WcReG9oH@Ip`tC=g@iYo)lBpfn!OINe`aU5nyJlc~m1M>F%-yDT+-*oL97ZpR3FJp7EW73= zJ1(YtHz#%pvP()oiIK9 z(=g8IX@hY++{`>5Q$@XR7b?446imGe_f%GazEkhZW%q>f1)MUE8DEe~@ifZ#0`6^6 zgYg9eshBoGPm=`!f{0F-#xF2>>PsPg3M6&WT({h3^u#@FEOzq+F$|>!;fM1DPePfa z1&Qms<8B(Cg4)$eOk4*aFe>1XR$)FZlbE;VqZB)SB`1N!1biIuVAg$0-crI=FB&q zt`n8r4mVGiFv^4=q7%k=J$_x?A$P0Y0driRHu_RA^>av(U+^Cw-6<;Iw$z<&dfMXV#Hyi8bv<#a@18NPhnr7LjGnmn!DHR@ z)Ci=6@WVN=AE2iKK?3>F37fum89iy&gduHp(^Dt-0(JJB&s^c=#GZvR?-8a!essck zDuD{r%W<<{mz$m%P`hf$3ZO)G>85srDWr#hR4WK^3h65#>Fqy(kbGRvJ=k|8E{sn{N-qi$H#46%4g=vuCv3MgH!9$!??*t& zl2|wwebUqpawoFe;pSBm#;XjYVtUlgs|o_`LJiP{_=LG>j5=dWQ zX8Jg>^-$&q5(}h7#B{FlbeuAiZhAUuRFI^y>j=hI3j)_IhfMA8V|2pwG|{+2&S7M` zF&;NOVZadC-3n#;c)Cf3C%rcaWBeMFUR^ZD_!1zgr$cxIPeq`)L=ZTKahXxU2r8yp zs6ag(_XIT1sDR6EwQ+|?G&*4_Xo*3o&j{SR*S$c>l32LxdO}a#1PPQyCoH>Dp(lMt z;C67aQ6{H?R)+C(D!UYrG6jL#Qu|EpICt2@FwVs^7D%O%9jAguAnARkew!7hr^nH3 zGbJWYPj?w7#_4IRn-kjrntlA!`B(#bC&sCuA10mpOK#Gw1(I$j&Z|_p_1zOjPuy1l z$AF~I2-(zB8D*5o-5NC*Wpa#vg%n=N8V>O*OSkr?R`w%^lVOX^6yvM57a?g4u>~Ze|WeAg@a-+$?B=siMxF zV|+i5vLq(%`|-Y}cAOJy22ZEXp4$jzKq{5&xJkDgNIpJupm9Ci%zVks^|Up5N(f(J z-SUai6Zg#7+^B$?bRU`8wWG58E#~-4LEt=HzHwq)-|a9?jO&(vW74S;;~1~Uq|>L3 zG>Y+;jmEh-UJYe-mRPuL{*+Mx_kCs-7+)K{sd^(n{l1+EOuHZ5ZdK%3ScIK9k$#cc5*3 zTu+S~<2Sl_mHV+i@X$!i=$Eys5#?i#l(SO4>!Y1Q-xbJ$V z8P~(T3H;1-3+JM5Hr>Kax>itTp)ih76`e4~Q$QLg2wd&xo)2BOrrv>Tjqdf zA1Aia@RTLYhw+qWc;cMcV(955i3OBJCoHBh(35&!&X3PEjJKp>`VHC^2}XzOmJCcf z)i&o22OCcZ%Aym-csp#)LP6ks<`f`}6C{qOJrL1yLE@a)-Kd>bpPQMPrgpg!TNvYC z11Tv8T(_iw@iBtL?YpT!DszeHAdq~V*j_gkbb%bVxn#EudQxp?3SyWFW)z47a7B|CsdCk*K{qbF{T z?=pJgkm}uhW`C4kzYyYl=5t`&$7hy;C-s@!-@reKq4c&?U!6vn3Wgg`$4$D|Ozo1C zG6$e`(*=S1@@1*19oKgYK+d}o6Gl~Z!Wb{a%+#83`!0z<)Y<1!F&zbx){L7fBcKBP ze}Fi~JHla%k*v6>G6P6ug2d_Ra;RXNAaQrm3xV_pLE>)LvW*Jz1TjpR-x(EfwTqkD zaXcM1J~N%lZa;**!libLP`iCDwR_jiKg~n!5)xAe#nVfezOw~^>$|pKT>Vp=ifJ;C z)Y)^&ydJ;4R$}2MU324~xZB!~jDOkcFlz?${Vq6E0 zGOkAv!!+LDR=bDXT+bk*0`800tKIxlDQc%)j#EK5OgeSZdDMJ54y2?YaIWV~s9>ld zadSMwI5Dnv4;wvk9ee~xdRv=LF@6BMWrZMcoAVksWp+1u8bD=t!sv;cDsMqggCrKN zc9}*`+_xcL8x>?wJiUtfH1Wq7%mWG$5&CRW}`mbTd|yK5cM)*9s}=?LT)Hz1#Rq z?yoHt85MM<7~c#PJR%4HjZPTjYr(ksOm5#*fpH(7nUCqKcVgTe9|oSL3qM?T>%h~? zf`lKV6UO)i!xPtc<;K%-Gqb>r@yD>G%7h<|@!_c53_${Dbix?#ffiB6dJBd03y_k6 zz&&#og7GT^iBrMbK=Sb_Nzgn-V&c5Y95)pl1(JH-%c$BN!j@Vo2wb;(jDo4dNT*`D z4uNbJ1a9BOpaNAUx24`OdgA<3w$W3Z;%P3B#tQ=XeD|HvQx__xxSM~faq~}yjm9}A zcB^q>+#GL%Zc!(8F~#^EHz&3Pb37rja8qR-^i(NG_%S+RTj~urPj|6VCZ~d0H)Rfl zp4JIJoPXMDRKR_c^|Voe?&7f8Z85dO_#{*?6O2DC2%Il?88oY7)tkSt?5YunI(u$Q zrHo_c-XrWYD#)Z5zYDdi5d^Mw4;r4hnOTMG^jinctMrD()%y;icq#!oR|>+}6w;^Q zN&OR7yFBBcl&>(2_c1)_O>!7gQ}CotjGIsI14*6zB^2Y`u$smQLYzW64kY#CoPRoE zybAX=sU7s><1o4#o;b!A0I5=#<~sNUkfyjGWxDx-uTAZ^IX=Yr0&YvCK?R$ICxjQB zunw-nEJ*wxfkE14d;zEN&PZvV!~{V^CoH?!KvIXnxx-Cv4r9OB=Gjzs%S`RKzuvjW z%@=HT<7qlHzD4)}Wzh*!!DQnwxNSb(%@^bw#yO4GgYgZ*4=5ve`W(NmUZod>)Zcg& zZf4%#=2co4uaZS&S8i0meLwzt<9a$#F%2+`bK5-Ejqx53c9CSoP2b-E$;V;*0wf=Y zan$&N4pi+%po8_kfb#_%(ZT8qxMzess6eygZXp-D)h>paskP(u^p2ZX$%YE%OZ5R7 zov42M8xeZmD)4cKUm_57hrOworWqA*%53H4=~8Z(iA^izRg9QQ7icVO!^u|Ug5`>l%(lK=KNQ~O1>?n%6}MZK0x2a(+#EmM z)QMDp~+1+CF#7(*vfRvC}uA^eQ m*UdkDXjGt2v0*$d!~pPdVzZ%wUBVB-i%wWf{{oVa6Z^w55&aZVKn1j*5=1^F34Zw^0bEcPMH3ZJl%;?u6_b#VU`RAy5QvB691@O# z%Ax|b;6jicTtFdugeZ$kP_Zr0R%`{UP%GAoRzT6-^LD0Zem{4fKjcsDnYnXk=DyGS z=1|il7W-A~f9GuZ_lj6fNyYf0oYDvK%X13ztL`l=A3ZUZld3GtsjNs9OsKBTNflO> z-dB}VRg|hqRg_h9DX6YK45ae~p(lm(;%Tv1LJ;~=NM&clVl{%0P9ZI77K_yh!kHA( z_U5tJ20=KBLi(mfEY=_h0F6#q?bsCVhZUb7+)y}aSCY%$k`|e z0F6!@J!T4cQ-=>`qUE@_n6p2}{7QD!!UGzdFb=l8i3(n_dcf*_niA)RGZz-{wC z0_hQnr2`ey7^vU{K>+#D2~)vbs9={MAUp!;VK}iig1|9;#CW>Cf*6+FV529lTV4TD zLSo6KVp;`dP7(xe7Tk}5)d&)&%soJQ)Fq}Rm~?f5#7*CMMwyU%bi!2dJ&+m%f$Nr8 zP-Z_tYAOGQ#q_GtQyzs>iQ3h=WcReG9oH@Ip`tC=g@iYo)lBpfn!OINe`aU5nyJlc~m1M>F%-yDT+-*oL97ZpR3FJp7EW73= zJ1(YtHz#%pvP()oiIK9 z(=g8IX@hY++{`>5Q$@XR7b?446imGe_f%GazEkhZW%q>f1)MUE8DEe~@ifZ#0`6^6 zgYg9eshBoGPm=`!f{0F-#xF2>>PsPg3M6&WT({h3^u#@FEOzq+F$|>!;fM1DPePfa z1&Qms<8B(Cg4)$eOk4*aFe>1XR$)FZlbE;VqZB)SB`1N!1biIuVAg$0-crI=FB&q zt`n8r4mVGiFv^4=q7%k=J$_x?A$P0Y0driRHu_RA^>av(U+^Cw-6<;Iw$z<&dfMXV#Hyi8bv<#a@18NPhnr7LjGnmn!DHR@ z)Ci=6@WVN=AE2iKK?3>F37fum89iy&gduHp(^Dt-0(JJB&s^c=#GZvR?-8a!essck zDuD{r%W<<{mz$m%P`hf$3ZO)G>85srDWr#hR4WK^3h65#>Fqy(kbGRvJ=k|8E{sn{N-qi$H#46%4g=vuCv3MgH!9$!??*t& zl2|wwebUqpawoFe;pSBm#;XjYVtUlgs|o_`LJiP{_=LG>j5=dWQ zX8Jg>^-$&q5(}h7#B{FlbeuAiZhAUuRFI^y>j=hI3j)_IhfMA8V|2pwG|{+2&S7M` zF&;NOVZadC-3n#;c)Cf3C%rcaWBeMFUR^ZD_!1zgr$cxIPeq`)L=ZTKahXxU2r8yp zs6ag(_XIT1sDR6EwQ+|?G&*4_Xo*3o&j{SR*S$c>l32LxdO}a#1PPQyCoH>Dp(lMt z;C67aQ6{H?R)+C(D!UYrG6jL#Qu|EpICt2@FwVs^7D%O%9jAguAnARkew!7hr^nH3 zGbJWYPj?w7#_4IRn-kjrntlA!`B(#bC&sCuA10mpOK#Gw1(I$j&Z|_p_1zOjPuy1l z$AF~I2-(zB8D*5o-5NC*Wpa#vg%n=N8V>O*OSkr?R`w%^lVOX^6yvM57a?g4u>~Ze|WeAg@a-+$?B=siMxF zV|+i5vLq(%`|-Y}cAOJy22ZEXp4$jzKq{5&xJkDgNIpJupm9Ci%zVks^|Up5N(f(J z-SUai6Zg#7+^B$?bRU`8wWG58E#~-4LEt=HzHwq)-|a9?jO&(vW74S;;~1~Uq|>L3 zG>Y+;jmEh-UJYe-mRPuL{*+Mx_kCs-7+)K{sd^(n{l1+EOuHZ5ZdK%3ScIK9k$#cc5*3 zTu+S~<2Sl_mHV+i@X$!i=$Eys5#?i#l(SO4>!Y1Q-xbJ$V z8P~(T3H;1-3+JM5Hr>Kax>itTp)ih76`e4~Q$QLg2wd&xo)2BOrrv>Tjqdf zA1Aia@RTLYhw+qWc;cMcV(955i3OBJCoHBh(35&!&X3PEjJKp>`VHC^2}XzOmJCcf z)i&o22OCcZ%Aym-csp#)LP6ks<`f`}6C{qOJrL1yLE@a)-Kd>bpPQMPrgpg!TNvYC z11Tv8T(_iw@iBtL?YpT!DszeHAdq~V*j_gkbb%bVxn#EudQxp?3SyWFW)z47a7B|CsdCk*K{qbF{T z?=pJgkm}uhW`C4kzYyYl=5t`&$7hy;C-s@!-@reKq4c&?U!6vn3Wgg`$4$D|Ozo1C zG6$e`(*=S1@@1*19oKgYK+d}o6Gl~Z!Wb{a%+#83`!0z<)Y<1!F&zbx){L7fBcKBP ze}Fi~JHla%k*v6>G6P6ug2d_Ra;RXNAaQrm3xV_pLE>)LvW*Jz1TjpR-x(EfwTqkD zaXcM1J~N%lZa;**!libLP`iCDwR_jiKg~n!5)xAe#nVfezOw~^>$|pKT>Vp=ifJ;C z)Y)^&ydJ;4R$}2MU324~xZB!~jDOkcFlz?${Vq6E0 zGOkAv!!+LDR=bDXT+bk*0`800tKIxlDQc%)j#EK5OgeSZdDMJ54y2?YaIWV~s9>ld zadSMwI5Dnv4;wvk9ee~xdRv=LF@6BMWrZMcoAVksWp+1u8bD=t!sv;cDsMqggCrKN zc9}*`+_xcL8x>?wJiUtfH1Wq7%mWG$5&CRW}`mbTd|yK5cM)*9s}=?LT)Hz1#Rq z?yoHt85MM<7~c#PJR%4HjZPTjYr(ksOm5#*fpH(7nUCqKcVgTe9|oSL3qM?T>%h~? zf`lKV6UO)i!xPtc<;K%-Gqb>r@yD>G%7h<|@!_c53_${Dbix?#ffiB6dJBd03y_k6 zz&&#og7GT^iBrMbK=Sb_Nzgn-V&c5Y95)pl1(JH-%c$BN!j@Vo2wb;(jDo4dNT*`D z4uNbJ1a9BOpaNAUx24`OdgA<3w$W3Z;%P3B#tQ=XeD|HvQx__xxSM~faq~}yjm9}A zcB^q>+#GL%Zc!(8F~#^EHz&3Pb37rja8qR-^i(NG_%S+RTj~urPj|6VCZ~d0H)Rfl zp4JIJoPXMDRKR_c^|Voe?&7f8Z85dO_#{*?6O2DC2%Il?88oY7)tkSt?5YunI(u$Q zrHo_c-XrWYD#)Z5zYDdi5d^Mw4;r4hnOTMG^jinctMrD()%y;icq#!oR|>+}6w;^Q zN&OR7yFBBcl&>(2_c1)_O>!7gQ}CotjGIsI14*6zB^2Y`u$smQLYzW64kY#CoPRoE zybAX=sU7s><1o4#o;b!A0I5=#<~sNUkfyjGWxDx-uTAZ^IX=Yr0&YvCK?R$ICxjQB zunw-nEJ*wxfkE14d;zEN&PZvV!~{V^CoH?!KvIXnxx-Cv4r9OB=Gjzs%S`RKzuvjW z%@=HT<7qlHzD4)}Wzh*!!DQnwxNSb(%@^bw#yO4GgYgZ*4=5ve`W(NmUZod>)Zcg& zZf4%#=2co4uaZS&S8i0meLwzt<9a$#F%2+`bK5-Ejqx53c9CSoP2b-E$;V;*0wf=Y zan$&N4pi+%po8_kfb#_%(ZT8qxMzess6eygZXp-D)h>paskP(u^p2ZX$%YE%OZ5R7 zov42M8xeZmD)4cKUm_57hrOworWqA*%53H4=~8Z(iA^izRg9QQ7icVO!^u|Ug5`>l%(lK=KNQ~O1>?n%6}MZK0x2a(+#EmM z)QMDp~+1+CF#7(*vfRvC}uA^eQ m*UdkDXjGt2v0*$d!~pPdVzZ%wUBVB-i%wWf{{oVa6Z;Rx1b` zQn_%490EsjOjUkG4ml*hAipH1RORcQktJC!4x0U@-}80PXoLDYjUWvh_FD_;cL%T= z50tfT7`3)Z*R43|(N+=%je5{a$uCw{OMb1m=9m4=l@cv)RyX{jUm}0Ax?WnTT)k>d zsB~+iyjEGK_MKWMXmz8s-w0)MFRZ6=LY*MpCg4S+xxYh=sFw!adPu>;sMCiEmc4$5 zf<}Y3!ykfH+$C6~VUw8BJms0DW$STa&CdKf~Eipnc%B}#+4Vccwjp&KP-3j0d4i7K0I5RU3` z!hVmcVLM2pdm-<{y(o?1E-VI+f_7@StX8g6{0%DFJ#6Y7^fkCw8{}CluCLG;ARqcIEl9)n4{%>l7!uqwSy>TLz1(wgG~|&>y5CNCh-H}Lqv4;cdRbI zEbwoI#{~YT@Ku4Om&9O3V5IPzz-JkedIJAZI4AI5h3^V{a##%B7x|I7kIzzOyQw!scC&P5KRc$#G_z>>2XgoX)>%HOt1#@SF~6b$snyXkoxCK-8D&x`B8hRB5!;dKz+Bsni@YAOOhQVNVP2VOe7?M0$fjqle?=Puf|M-@&RY_yJ zGjWW^U_I;Q$4_Tzx;Av2Y`-{~_D@#!M1$_hWrLPq6jU6?hW|RO&AiS1i5jg=Sg|Lv z-L62Q7k_ZaObc#_STCUQ% ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# 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) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + 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) + + log("Time: ", timeW) + if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS + timeW =0 + statef=land + } else { + if(timeW >= (WAIT_TIMEOUT / 2)){ + #uav_moveto(4.0,0.0) + } else { + #uav_moveto(4.0,0.0) + #uav_moveto(0.0,4.0) + } + timeW = timeW+1 + } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 300 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS, hexagon) + #barrier_set(ROBOTS, land); + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index f968dfe..41f9b51 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -114,7 +114,8 @@ namespace rosbzz_node{ /*sleep for the mentioned loop rate*/ timer_step+=1; maintain_pos(timer_step); - + + std::cout<< "HOME: " << home[0] << ", " << home[1] < Date: Tue, 9 May 2017 18:25:37 -0400 Subject: [PATCH 14/26] first working implementation of users vstig --- include/buzz_utility.h | 1 + include/buzzuav_closures.h | 2 +- script/testflocksim.bzz | 30 ++++++++--------- script/teststig.bzz | 2 +- src/buzz_utility.cpp | 67 ++++++++++++++++++++++++++++++++++++-- src/buzzuav_closures.cpp | 21 ++++++++++-- 6 files changed, 100 insertions(+), 23 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index fb744e5..aa5cb2a 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -39,6 +39,7 @@ void update_users(); int make_table(buzzobj_t* t); int buzzusers_add(int id, double latitude, double longitude, double altitude); int buzzusers_reset(); + int compute_users_rb(); void in_msg_append(uint64_t* payload); diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 2e62d32..822c391 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -84,7 +84,7 @@ int buzzuav_update_battery(buzzvm_t vm); * Updates current position in Buzz */ int buzzuav_update_currentpos(buzzvm_t vm); - +int buzzuav_adduserRB(buzzvm_t vm); /* * Updates flight status and rc command in Buzz, put it in a tabel to acess it * use flight.status for flight status diff --git a/script/testflocksim.bzz b/script/testflocksim.bzz index 616de9c..79c58d8 100644 --- a/script/testflocksim.bzz +++ b/script/testflocksim.bzz @@ -141,10 +141,13 @@ function land() { } } -function table_print(t) { - foreach(t, function(key, value) { - log(key, " -> ", value) - }) +function users_print(t) { + if(size(t)>0) { + foreach(t, function(id, tab) { + log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo) + add_user_rb(id,tab.la,tab.lo) + }) + } } ######################################## @@ -157,10 +160,9 @@ function table_print(t) { function init() { s = swarm.create(1) s.join() - v = stigmergy.create(5) + vt = stigmergy.create(5) t = {} - v.put("p",t) - v.put("u",1) + vt.put("p",t) statef=idle CURSTATE = "IDLE" } @@ -211,22 +213,18 @@ neighbors.listen("cmd", statef() log("Current state: ", CURSTATE) log("Swarm size: ",ROBOTS) - #log("User position: ", users.range) # Read a value from the structure - #t = v.get("p") log(users) - table_print(users) if(size(users)>0){ - tmp = {2 3} - v.put("p", tmp) - v.put("u",2) + #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 ", v.size(), " elements") - table_print(v.get("p")) - log(v.get("u")) + log("The vstig has ", vt.size(), " elements") + users_print(vt.get("p")) } # Executed once when the robot (or the simulator) is reset. diff --git a/script/teststig.bzz b/script/teststig.bzz index d3c4139..dc6c89e 100644 --- a/script/teststig.bzz +++ b/script/teststig.bzz @@ -23,7 +23,7 @@ function step() { log("The vstig has ", v.size(), " elements") log(v.get("u")) if (id==1) { - tmp = { } + tmp = { .x=3} v.put("p",tmp) v.put("u",2) } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index dfe19d6..024227c 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -17,7 +17,7 @@ namespace buzz_utility{ static char* BO_FNAME = 0; static uint8_t* BO_BUF = 0; static buzzdebug_t DBG_INFO = 0; - static uint8_t MSG_SIZE = 50; // Only 100 bytes of Buzz messages every step + static uint8_t MSG_SIZE = 250; // 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; @@ -57,6 +57,61 @@ 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() { @@ -92,13 +147,13 @@ namespace buzz_utility{ 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_pushs(VM, buzzvm_string_register(VM, "dataG", 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_pushs(VM, buzzvm_string_register(VM, "dataG", 1)); buzzvm_pusht(VM); buzzobj_t data = buzzvm_stack_at(VM, 1); buzzvm_tput(VM); @@ -314,6 +369,9 @@ 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); + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_user_rb", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_adduserRB)); + buzzvm_gstore(VM); return VM->state; } @@ -350,6 +408,9 @@ 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); + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_user_rb", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); return VM->state; } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index afaf1a9..4f98aed 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -108,8 +108,8 @@ namespace buzzuav_closures{ float dx = buzzvm_stack_at(vm, 2)->f.value; double d = sqrt(dx*dx+dy*dy); //range goto_pos[0]=dx; - goto_pos[1]=dy; - goto_pos[2]=height; + goto_pos[1]=dy; + goto_pos[2]=height; /*double b = atan2(dy,dx); //bearing printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); gps_from_rb(d, b, goto_pos); @@ -119,6 +119,23 @@ namespace buzzuav_closures{ return buzzvm_ret0(vm); } + int buzzuav_adduserRB(buzzvm_t vm) { + buzzvm_lnum_assert(vm, 3); + buzzvm_lload(vm, 1); /* longitude */ + buzzvm_lload(vm, 2); /* latitude */ + buzzvm_lload(vm, 3); /* id */ + 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; + int uid = buzzvm_stack_at(vm, 3)->i.value; + + printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, lat, lon); + + return buzzvm_ret0(vm); + } + /*----------------------------------------/ / Buzz closure to go directly to a GPS destination from the Mission Planner /----------------------------------------*/ From b095536db64f90f3ef8e6dd0bdb10a2b1588acdd Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Tue, 9 May 2017 19:05:57 -0400 Subject: [PATCH 15/26] in messages with local buff --- script/update_sim/testflockfev1.bzz | 208 ++++++++++++++++++++++++++++ script/update_sim/testflockfev2.bzz | 208 ++++++++++++++++++++++++++++ script/update_sim/testflockfev3.bzz | 208 ++++++++++++++++++++++++++++ script/update_sim/testflockfev4.bzz | 208 ++++++++++++++++++++++++++++ script/update_sim/testflockfev5.bzz | 208 ++++++++++++++++++++++++++++ script/update_sim/testflockfev6.bzz | 208 ++++++++++++++++++++++++++++ script/update_sim/testflockfev7.bzz | 208 ++++++++++++++++++++++++++++ src/buzz_utility.cpp | 71 ++++++---- 8 files changed, 1498 insertions(+), 29 deletions(-) create mode 100644 script/update_sim/testflockfev1.bzz create mode 100644 script/update_sim/testflockfev2.bzz create mode 100644 script/update_sim/testflockfev3.bzz create mode 100644 script/update_sim/testflockfev4.bzz create mode 100644 script/update_sim/testflockfev5.bzz create mode 100644 script/update_sim/testflockfev6.bzz create mode 100644 script/update_sim/testflockfev7.bzz diff --git a/script/update_sim/testflockfev1.bzz b/script/update_sim/testflockfev1.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev1.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# 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) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + 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) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev2.bzz b/script/update_sim/testflockfev2.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev2.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# 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) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + 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) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev3.bzz b/script/update_sim/testflockfev3.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev3.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# 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) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + 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) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev4.bzz b/script/update_sim/testflockfev4.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev4.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# 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) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + 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) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev5.bzz b/script/update_sim/testflockfev5.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev5.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# 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) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + 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) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev6.bzz b/script/update_sim/testflockfev6.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev6.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# 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) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + 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) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev7.bzz b/script/update_sim/testflockfev7.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev7.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# 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) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + 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) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 024227c..2492e6d 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -19,8 +19,8 @@ namespace buzz_utility{ static buzzdebug_t DBG_INFO = 0; static uint8_t MSG_SIZE = 250; // 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; - + static int Robot_id = 0; + static std::vector IN_MSG; std::map< int, Pos_struct> users_map; /****************************************/ @@ -228,41 +228,55 @@ namespace buzz_utility{ void in_msg_append(uint64_t* payload){ - /* Go through messages and add them to the FIFO */ + /* Go through messages and append them to the vector */ uint16_t* data= u64_cvt_u16((uint64_t)payload[0]); /*Size is at first 2 bytes*/ uint16_t size=data[0]*sizeof(uint64_t); delete[] data; uint8_t* pl =(uint8_t*)malloc(size); - memset(pl, 0,size); - /* Copy packet into temporary buffer */ - memcpy(pl, payload ,size); - /*size and robot id read*/ - size_t tot = sizeof(uint32_t); - /* Go through the messages until there's nothing else to read */ - uint16_t unMsgSize=0; - - /*Obtain Buzz messages only when they are present*/ - do { - /* Get payload size */ - unMsgSize = *(uint16_t*)(pl + tot); - tot += sizeof(uint16_t); - /* Append message to the Buzz input message queue */ - if(unMsgSize > 0 && unMsgSize <= size - tot ) { - buzzinmsg_queue_append(VM, - buzzmsg_payload_frombuffer(pl +tot, unMsgSize)); - tot += unMsgSize; - } - }while(size - tot > sizeof(uint16_t) && unMsgSize > 0); - free(pl); + /* Copy packet into temporary buffer */ + memcpy(pl, payload ,size); + IN_MSG.push_back(pl); } + + void in_message_process(){ + while(!IN_MSG.empty()){ + uint8_t* first_INmsg = (uint8_t*)IN_MSG.front(); + /* Go through messages and append them to the FIFO */ + uint16_t* data= u64_cvt_u16((uint64_t)first_INmsg[0]); + /*Size is at first 2 bytes*/ + uint16_t size=data[0]*sizeof(uint64_t); + delete[] data; + /*size and robot id read*/ + size_t tot = sizeof(uint32_t); + /* Go through the messages until there's nothing else to read */ + uint16_t unMsgSize=0; + /*Obtain Buzz messages push it into queue*/ + do { + /* Get payload size */ + unMsgSize = *(uint16_t*)(first_INmsg + tot); + tot += sizeof(uint16_t); + /* Append message to the Buzz input message queue */ + if(unMsgSize > 0 && unMsgSize <= size - tot ) { + buzzinmsg_queue_append(VM, + buzzmsg_payload_frombuffer(first_INmsg +tot, unMsgSize)); + tot += unMsgSize; + } + }while(size - tot > sizeof(uint16_t) && unMsgSize > 0); + IN_MSG.erase(IN_MSG.begin()); + free(first_INmsg); + } + /* Process messages VM call*/ + buzzvm_process_inmsgs(VM); + } /***************************************************/ /*Obtains messages from buzz out message Queue*/ /***************************************************/ uint64_t* obt_out_msg(){ - + /* Process out messages */ + buzzvm_process_outmsgs(VM); 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*/ @@ -699,8 +713,8 @@ static int create_stig_tables() { } void buzz_script_step() { - /* Process messages */ - buzzvm_process_inmsgs(VM); + /*Process available messages*/ + in_message_process(); /*Update sensors*/ update_sensors(); /* Call Buzz step() function */ @@ -710,8 +724,7 @@ static int create_stig_tables() { buzz_error_info()); 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; From cbdc5a6db2c7cd471c9661d4133cd1ff5406ceec Mon Sep 17 00:00:00 2001 From: isvogor Date: Tue, 9 May 2017 19:48:38 -0400 Subject: [PATCH 16/26] open loop --- include/roscontroller.h | 1 + launch/rosbuzz-testing-sitl.launch | 2 +- script/testsolo.basm | 1560 +++++++++++++++++----------- script/testsolo.bdb | Bin 49136 -> 65144 bytes script/testsolo.bdbg | Bin 49136 -> 65144 bytes script/testsolo.bo | Bin 2969 -> 3809 bytes script/testsolo.bzz | 5 +- src/roscontroller.cpp | 18 +- 8 files changed, 945 insertions(+), 641 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index 43cb117..b3adb2b 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -70,6 +70,7 @@ private: double DEFAULT_REFERENCE_LONGITUDE = -73.636075; double cur_pos[3]; + double target[3]; double home[3]; double cur_rel_altitude; uint64_t payload; diff --git a/launch/rosbuzz-testing-sitl.launch b/launch/rosbuzz-testing-sitl.launch index 6d3da79..c09d2c5 100644 --- a/launch/rosbuzz-testing-sitl.launch +++ b/launch/rosbuzz-testing-sitl.launch @@ -27,7 +27,7 @@ - + diff --git a/script/testsolo.basm b/script/testsolo.basm index 3ec341b..869b93d 100644 --- a/script/testsolo.basm +++ b/script/testsolo.basm @@ -1,4 +1,21 @@ -!83 +!95 +'math +'vec2 +'new +'x +'y +'newp +'cos +'sin +'length +'sqrt +'angle +'atan2 +'norm +'add +'sub +'scale +'dot 'updated 'update_ack 'update_no @@ -12,26 +29,21 @@ 'EPSILON 'lj_magnitude 'lj_vector -'math -'vec2 -'newp 'distance 'azimuth 'lj_sum -'add 'hexagon 'statef 'HEXAGON 'map 'reduce -'new 'count -'scale 'log 'Time: 'timeW 'WAIT_TIMEOUT 'land +'uav_moveto 'BARRIER_VSTIG 'barrier_set 'barrier_wait @@ -83,99 +95,369 @@ 'reset 'destroy - pushs 3 - pushcn @__label_1 - gstore - pushs 11 - pushcn @__label_2 - gstore - pushs 12 - pushcn @__label_3 - gstore - pushs 18 - pushcn @__label_4 - gstore pushs 20 - pushcn @__label_5 + pushcn @__label_10 gstore - pushs 34 + pushs 28 + pushcn @__label_11 + gstore + pushs 29 pushcn @__label_12 gstore - pushs 39 + pushs 32 + pushcn @__label_13 + gstore + pushs 33 pushcn @__label_14 gstore - pushs 35 - pushcn @__label_15 - gstore - pushs 45 - pushcn @__label_20 - gstore - pushs 47 + pushs 46 pushcn @__label_21 gstore - pushs 32 + pushs 51 + pushcn @__label_23 + gstore + pushs 47 pushcn @__label_24 gstore - pushs 62 - pushcn @__label_27 + pushs 57 + pushcn @__label_29 gstore - pushs 66 - pushcn @__label_28 + pushs 59 + pushcn @__label_30 gstore - pushs 81 - pushcn @__label_48 + pushs 43 + pushcn @__label_33 gstore - pushs 82 - pushcn @__label_49 + pushs 74 + pushcn @__label_36 + gstore + pushs 78 + pushcn @__label_37 + gstore + pushs 93 + pushcn @__label_57 + gstore + pushs 94 + pushcn @__label_58 gstore nop @__label_0 - pushs 0 |8,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 1 |8,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 0 |4,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |4,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |4,5,/home/ubuntu/buzz/src/include/vec2.bzz + pusht |4,13,/home/ubuntu/buzz/src/include/vec2.bzz + tput |4,14,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |12,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |12,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |12,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |12,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 2 |12,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_1 |12,16,/home/ubuntu/buzz/src/include/vec2.bzz + tput |14,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |22,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |22,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |22,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |22,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 5 |22,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_2 |22,17,/home/ubuntu/buzz/src/include/vec2.bzz + tput |27,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |34,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |34,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |34,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |34,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 8 |34,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_3 |34,19,/home/ubuntu/buzz/src/include/vec2.bzz + tput |36,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |43,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |43,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |43,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |43,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 10 |43,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_4 |43,18,/home/ubuntu/buzz/src/include/vec2.bzz + tput |45,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |52,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |52,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |52,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |52,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 12 |52,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_5 |52,17,/home/ubuntu/buzz/src/include/vec2.bzz + tput |58,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |66,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |66,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |66,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |66,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 13 |66,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_6 |66,16,/home/ubuntu/buzz/src/include/vec2.bzz + tput |71,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |79,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |79,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |79,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |79,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 14 |79,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_7 |79,16,/home/ubuntu/buzz/src/include/vec2.bzz + tput |84,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |92,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |92,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |92,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |92,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 15 |92,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_8 |92,18,/home/ubuntu/buzz/src/include/vec2.bzz + tput |97,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |105,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |105,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |105,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |105,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 16 |105,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_9 |105,16,/home/ubuntu/buzz/src/include/vec2.bzz + tput |107,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 17 |8,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 18 |8,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gstore |8,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 2 |9,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 19 |9,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushi 0 |9,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gstore |9,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 6 |14,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 23 |14,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushf 3.0 |14,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gstore |14,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 7 |15,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 8 |15,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |15,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 25 |15,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gstore |15,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 9 |18,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 26 |18,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushf 10.0 |18,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gstore |19,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 10 |19,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 27 |19,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushf 18.0 |19,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gstore |20,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 33 |72,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 1 |72,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |72,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 31 |95,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 300 |95,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |95,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 30 |96,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |96,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |96,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 45 |71,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |71,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |71,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 42 |94,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 300 |94,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |94,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |95,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |95,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |95,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz @__exitpoint done @__label_1 - pushs 4 |11,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pusht |13,11,/home/ubuntu/buzz/src/include/vec2.bzz + dup |13,11,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |13,12,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |13,17,/home/ubuntu/buzz/src/include/vec2.bzz + tput |13,17,/home/ubuntu/buzz/src/include/vec2.bzz + dup |13,17,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |13,20,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |13,26,/home/ubuntu/buzz/src/include/vec2.bzz + tput |13,26,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |13,27,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |14,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_2 + pusht |24,4,/home/ubuntu/buzz/src/include/vec2.bzz + dup |24,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |24,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |24,11,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |24,17,/home/ubuntu/buzz/src/include/vec2.bzz + gload |24,17,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 6 |24,18,/home/ubuntu/buzz/src/include/vec2.bzz + tget |24,21,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |24,23,/home/ubuntu/buzz/src/include/vec2.bzz + pushi 1 |24,24,/home/ubuntu/buzz/src/include/vec2.bzz + callc |24,24,/home/ubuntu/buzz/src/include/vec2.bzz + mul |24,24,/home/ubuntu/buzz/src/include/vec2.bzz + tput |24,24,/home/ubuntu/buzz/src/include/vec2.bzz + dup |24,24,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |25,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |25,11,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |25,17,/home/ubuntu/buzz/src/include/vec2.bzz + gload |25,17,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 7 |25,18,/home/ubuntu/buzz/src/include/vec2.bzz + tget |25,21,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |25,23,/home/ubuntu/buzz/src/include/vec2.bzz + pushi 1 |25,24,/home/ubuntu/buzz/src/include/vec2.bzz + callc |25,24,/home/ubuntu/buzz/src/include/vec2.bzz + mul |25,24,/home/ubuntu/buzz/src/include/vec2.bzz + tput |25,24,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |26,3,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |27,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_3 + pushs 0 |35,13,/home/ubuntu/buzz/src/include/vec2.bzz + gload |35,13,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 9 |35,14,/home/ubuntu/buzz/src/include/vec2.bzz + tget |35,18,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |35,20,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |35,21,/home/ubuntu/buzz/src/include/vec2.bzz + tget |35,23,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |35,26,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |35,27,/home/ubuntu/buzz/src/include/vec2.bzz + tget |35,29,/home/ubuntu/buzz/src/include/vec2.bzz + mul |35,29,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |35,32,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |35,33,/home/ubuntu/buzz/src/include/vec2.bzz + tget |35,35,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |35,38,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |35,39,/home/ubuntu/buzz/src/include/vec2.bzz + tget |35,40,/home/ubuntu/buzz/src/include/vec2.bzz + mul |35,40,/home/ubuntu/buzz/src/include/vec2.bzz + add |35,40,/home/ubuntu/buzz/src/include/vec2.bzz + pushi 1 |35,41,/home/ubuntu/buzz/src/include/vec2.bzz + callc |35,41,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |35,41,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |36,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_4 + pushs 0 |44,13,/home/ubuntu/buzz/src/include/vec2.bzz + gload |44,13,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 11 |44,14,/home/ubuntu/buzz/src/include/vec2.bzz + tget |44,19,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |44,21,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |44,22,/home/ubuntu/buzz/src/include/vec2.bzz + tget |44,23,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |44,26,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |44,27,/home/ubuntu/buzz/src/include/vec2.bzz + tget |44,28,/home/ubuntu/buzz/src/include/vec2.bzz + pushi 2 |44,29,/home/ubuntu/buzz/src/include/vec2.bzz + callc |44,29,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |44,29,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |45,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_5 + pushs 0 |53,14,/home/ubuntu/buzz/src/include/vec2.bzz + gload |53,14,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 8 |53,15,/home/ubuntu/buzz/src/include/vec2.bzz + tget |53,21,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |53,23,/home/ubuntu/buzz/src/include/vec2.bzz + pushi 1 |53,24,/home/ubuntu/buzz/src/include/vec2.bzz + callc |53,24,/home/ubuntu/buzz/src/include/vec2.bzz + lstore 2 |53,24,/home/ubuntu/buzz/src/include/vec2.bzz + pusht |55,4,/home/ubuntu/buzz/src/include/vec2.bzz + dup |55,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |55,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |55,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |55,11,/home/ubuntu/buzz/src/include/vec2.bzz + tget |55,13,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |55,16,/home/ubuntu/buzz/src/include/vec2.bzz + div |55,16,/home/ubuntu/buzz/src/include/vec2.bzz + tput |55,16,/home/ubuntu/buzz/src/include/vec2.bzz + dup |55,16,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |56,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |56,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |56,11,/home/ubuntu/buzz/src/include/vec2.bzz + tget |56,13,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |56,16,/home/ubuntu/buzz/src/include/vec2.bzz + div |56,16,/home/ubuntu/buzz/src/include/vec2.bzz + tput |56,16,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |57,3,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |58,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_6 + pusht |68,4,/home/ubuntu/buzz/src/include/vec2.bzz + dup |68,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |68,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |68,11,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |68,12,/home/ubuntu/buzz/src/include/vec2.bzz + tget |68,14,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |68,18,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |68,19,/home/ubuntu/buzz/src/include/vec2.bzz + tget |68,20,/home/ubuntu/buzz/src/include/vec2.bzz + add |68,20,/home/ubuntu/buzz/src/include/vec2.bzz + tput |68,20,/home/ubuntu/buzz/src/include/vec2.bzz + dup |68,20,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |69,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |69,11,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |69,12,/home/ubuntu/buzz/src/include/vec2.bzz + tget |69,14,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |69,18,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |69,19,/home/ubuntu/buzz/src/include/vec2.bzz + tget |69,20,/home/ubuntu/buzz/src/include/vec2.bzz + add |69,20,/home/ubuntu/buzz/src/include/vec2.bzz + tput |69,20,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |70,3,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |71,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_7 + pusht |81,4,/home/ubuntu/buzz/src/include/vec2.bzz + dup |81,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |81,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |81,11,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |81,12,/home/ubuntu/buzz/src/include/vec2.bzz + tget |81,14,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |81,18,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |81,19,/home/ubuntu/buzz/src/include/vec2.bzz + tget |81,20,/home/ubuntu/buzz/src/include/vec2.bzz + sub |81,20,/home/ubuntu/buzz/src/include/vec2.bzz + tput |81,20,/home/ubuntu/buzz/src/include/vec2.bzz + dup |81,20,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |82,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |82,11,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |82,12,/home/ubuntu/buzz/src/include/vec2.bzz + tget |82,14,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |82,18,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |82,19,/home/ubuntu/buzz/src/include/vec2.bzz + tget |82,20,/home/ubuntu/buzz/src/include/vec2.bzz + sub |82,20,/home/ubuntu/buzz/src/include/vec2.bzz + tput |82,20,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |83,3,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |84,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_8 + pusht |94,4,/home/ubuntu/buzz/src/include/vec2.bzz + dup |94,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |94,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |94,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |94,11,/home/ubuntu/buzz/src/include/vec2.bzz + tget |94,13,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |94,16,/home/ubuntu/buzz/src/include/vec2.bzz + mul |94,16,/home/ubuntu/buzz/src/include/vec2.bzz + tput |94,16,/home/ubuntu/buzz/src/include/vec2.bzz + dup |94,16,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |95,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |95,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |95,11,/home/ubuntu/buzz/src/include/vec2.bzz + tget |95,13,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |95,16,/home/ubuntu/buzz/src/include/vec2.bzz + mul |95,16,/home/ubuntu/buzz/src/include/vec2.bzz + tput |95,16,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |96,3,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |97,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_9 + lload 1 |106,11,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |106,12,/home/ubuntu/buzz/src/include/vec2.bzz + tget |106,14,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |106,18,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |106,19,/home/ubuntu/buzz/src/include/vec2.bzz + tget |106,21,/home/ubuntu/buzz/src/include/vec2.bzz + mul |106,21,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |106,25,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |106,26,/home/ubuntu/buzz/src/include/vec2.bzz + tget |106,28,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |106,32,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |106,33,/home/ubuntu/buzz/src/include/vec2.bzz + tget |106,34,/home/ubuntu/buzz/src/include/vec2.bzz + mul |106,34,/home/ubuntu/buzz/src/include/vec2.bzz + add |106,34,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |106,34,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |107,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_10 + pushs 21 |11,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |11,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 5 |11,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 22 |11,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz tget |11,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 0 |11,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 17 |11,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |11,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 2 |11,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 19 |11,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |11,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushi 2 |11,39,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz callc |11,39,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz ret0 |12,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_2 +@__label_11 lload 3 |23,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz lload 1 |23,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz div |23,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz @@ -195,38 +477,38 @@ ret1 |23,68,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz ret0 |24,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_3 - pushs 13 |28,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_12 + pushs 0 |28,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |28,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 14 |28,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 1 |28,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz tget |28,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 15 |28,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 5 |28,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz tget |28,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 11 |28,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 28 |28,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |28,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz lload 2 |28,41,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 16 |28,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 30 |28,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz tget |28,50,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 9 |28,58,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 26 |28,58,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |28,58,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 10 |28,67,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 27 |28,67,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |28,67,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushi 3 |28,68,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz callc |28,68,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz lload 2 |28,74,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 17 |28,75,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 31 |28,75,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz tget |28,82,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushi 2 |28,83,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz callc |28,83,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz ret1 |28,83,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz ret0 |29,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_4 - pushs 13 |33,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_13 + pushs 0 |33,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |33,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 14 |33,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 1 |33,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz tget |33,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 19 |33,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 13 |33,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz tget |33,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz lload 2 |33,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz lload 3 |33,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz @@ -235,31 +517,31 @@ ret1 |33,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz ret0 |34,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_5 - pushs 21 |38,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 20 |38,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_14 + pushs 34 |38,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 33 |38,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |38,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gstore |38,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 7 |39,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 22 |39,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |39,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 35 |39,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gstore |39,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 4 |41,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |41,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |41,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 23 |41,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 36 |41,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz tget |41,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 12 |41,37,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 29 |41,37,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |41,37,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushi 1 |41,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz callc |41,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 24 |41,39,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 37 |41,39,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz tget |41,45,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 18 |41,52,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 32 |41,52,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |41,52,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 13 |41,58,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 0 |41,58,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |41,58,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 14 |41,59,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 1 |41,59,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz tget |41,63,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 25 |41,64,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 2 |41,64,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz tget |41,67,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushf 0.0 |41,68,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushf 0.0 |41,73,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz @@ -268,595 +550,607 @@ pushi 2 |41,78,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz callc |41,78,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz lstore 1 |41,78,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 4 |42,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |42,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |42,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 26 |42,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 38 |42,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz tget |42,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushi 0 |42,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz callc |42,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushi 0 |42,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gt |42,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jumpz @__label_6 |42,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 13 |43,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_15 |42,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 0 |43,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |43,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 14 |43,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 1 |43,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz tget |43,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 27 |43,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 15 |43,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz tget |43,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz lload 1 |43,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushf 1.0 |43,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 4 |43,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |43,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |43,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 26 |43,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 38 |43,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz tget |43,48,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushi 0 |43,50,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz callc |43,50,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz div |43,50,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushi 2 |43,51,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz callc |43,51,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_6 |48,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 28 |48,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_15 |48,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |48,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |48,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 29 |48,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 30 |48,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 40 |48,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |48,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |48,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushi 2 |48,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz callc |48,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 30 |49,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |49,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |49,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 31 |49,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 42 |49,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |49,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gte |49,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jumpz @__label_8 |49,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 30 |50,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_17 |49,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |50,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushi 0 |50,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gstore |50,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 21 |51,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 32 |51,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |51,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 43 |51,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |51,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gstore |51,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jump @__label_9 |52,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_8 |52,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 30 |53,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_18 |52,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_17 |52,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |53,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |53,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 31 |53,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 42 |53,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |53,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz pushi 2 |53,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz div |53,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gte |53,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jumpz @__label_10 |53,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jump @__label_11 |55,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_10 |55,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_11 |58,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 30 |59,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 30 |59,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |59,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 1 |59,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - add |59,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |59,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_9 |60,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - ret0 |61,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - -@__label_12 - pushs 21 |79,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushl @__label_13 |79,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |81,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 36 |82,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 37 |82,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |82,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 38 |82,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |82,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 33 |82,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |82,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 1 |82,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |82,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |82,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - ret0 |83,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - -@__label_13 - pushs 35 |80,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |80,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - lload 1 |80,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - lload 2 |80,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |80,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |80,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - ret0 |81,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - -@__label_14 - pushs 36 |89,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |89,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 40 |89,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |89,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 41 |89,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |89,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 1 |89,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |89,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |89,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - ret0 |90,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - -@__label_15 - pushs 36 |98,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |98,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 42 |98,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |98,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 41 |98,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |98,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 1 |98,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |98,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 7 |99,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 43 |99,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |99,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 36 |100,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |100,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 44 |100,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |100,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |100,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |100,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - lload 1 |100,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gte |100,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jumpz @__label_16 |100,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 36 |101,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushnil |101,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |101,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - lload 2 |102,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |102,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |102,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jump @__label_17 |103,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_16 |103,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 30 |103,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |103,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 31 |103,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |103,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gte |103,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jumpz @__label_18 |103,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 36 |104,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushnil |104,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |104,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 21 |105,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 32 |105,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |105,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |105,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 30 |106,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |106,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |106,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_18 |108,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_17 |108,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 30 |108,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 30 |108,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |108,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 1 |108,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - add |108,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |108,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - ret0 |109,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - -@__label_20 - pushs 21 |114,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 45 |114,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |114,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |114,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 7 |115,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 46 |115,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |115,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - ret0 |117,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_19 |53,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 44 |54,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |54,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 0.03 |54,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 0.0 |54,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |54,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |54,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_20 |55,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_19 |55,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 44 |56,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |56,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 0.0 |56,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 0.03 |56,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |56,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |56,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_20 |57,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |58,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |58,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |58,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |58,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + add |58,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |58,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_18 |59,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |60,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz @__label_21 - pushs 7 |120,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 48 |120,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |120,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 21 |121,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 47 |121,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |121,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |121,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 28 |122,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |122,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 49 |122,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 50 |122,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |122,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 51 |122,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |122,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |122,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |122,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 28 |123,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |123,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 52 |123,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 53 |123,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |123,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 54 |123,37,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |123,45,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |123,46,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |123,46,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 50 |125,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |125,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 51 |125,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |125,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |125,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - eq |125,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 53 |125,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |125,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 54 |125,37,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |125,46,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 6 |125,64,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |125,64,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 6 |125,80,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |125,80,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushf 20.0 |125,81,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - div |125,85,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - sub |125,85,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gte |125,85,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - and |125,85,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jumpz @__label_22 |125,87,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 34 |126,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |126,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 55 |126,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |126,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 20 |126,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |126,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |126,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |126,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 39 |128,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |128,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |128,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |128,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jump @__label_23 |131,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_22 |131,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 28 |132,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |132,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 56 |132,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 6 |132,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |132,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |132,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |132,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 4 |133,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |133,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 5 |133,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |133,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 57 |133,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 22 |133,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |133,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |133,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 58 |134,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |134,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 6 |134,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |134,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 1 |134,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |134,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_23 |135,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - ret0 |136,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |78,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushl @__label_22 |78,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |80,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 48 |81,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 49 |81,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |81,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |81,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |81,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 45 |81,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |81,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |81,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |81,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |81,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |82,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_22 + pushs 47 |79,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |79,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |79,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |79,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |79,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |79,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |80,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_23 + pushs 48 |88,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |88,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 52 |88,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |88,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 53 |88,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |88,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |88,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |88,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |88,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |89,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz @__label_24 - pushs 7 |138,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 59 |138,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |138,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 21 |139,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 32 |139,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |139,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |139,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 28 |140,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |140,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 60 |140,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 50 |140,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |140,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 51 |140,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |140,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |140,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |140,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 50 |141,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |141,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 51 |141,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |141,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |141,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - eq |141,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 50 |141,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |141,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 51 |141,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |141,40,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 3 |141,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - eq |141,44,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - or |141,44,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jumpz @__label_25 |141,45,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 4 |142,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |142,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 5 |142,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |142,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 57 |142,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 21 |142,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |142,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |142,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 61 |143,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |143,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |143,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |143,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jump @__label_26 |145,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_25 |145,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 30 |146,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |146,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |146,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 36 |147,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushnil |147,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |147,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 21 |148,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 45 |148,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |148,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |148,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_26 |149,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - ret0 |150,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 48 |97,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |97,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 54 |97,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |97,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 53 |97,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |97,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |97,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |97,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |98,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 55 |98,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |98,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 48 |99,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |99,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 56 |99,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |99,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |99,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |99,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |99,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gte |99,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_25 |99,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 48 |100,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushnil |100,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |100,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |101,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |101,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |101,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_26 |102,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_25 |102,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |102,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |102,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 42 |102,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |102,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gte |102,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_27 |102,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 48 |103,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushnil |103,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |103,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |104,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 43 |104,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |104,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |104,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |105,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |105,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |105,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_27 |107,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_26 |107,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |107,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |107,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |107,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |107,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + add |107,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |107,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |108,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_27 - pushs 63 |154,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 64 |154,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |154,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 38 |154,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |154,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 1 |154,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 1 |154,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |154,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |154,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 63 |156,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |156,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 65 |156,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |156,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |156,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |156,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 21 |157,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 45 |157,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |157,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |157,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 7 |158,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 46 |158,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |158,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - ret0 |159,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_29 + pushs 34 |113,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 57 |113,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |113,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |113,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |114,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 58 |114,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |114,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |116,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_28 - pushs 50 |163,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |163,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 67 |163,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |163,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 22 |163,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - eq |163,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jumpz @__label_29 |163,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 28 |164,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |164,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 68 |164,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 1 |164,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |164,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 50 |165,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |165,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 67 |165,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |165,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tput |165,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 21 |166,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 47 |166,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |166,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |166,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 7 |167,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 48 |167,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |167,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 4 |168,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |168,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 5 |168,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |168,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 57 |168,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 22 |168,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |168,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |168,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jump @__label_30 |169,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_29 |169,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 50 |169,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |169,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 67 |169,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |169,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 21 |169,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - eq |169,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jumpz @__label_31 |169,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 28 |170,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_30 + pushs 24 |119,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 60 |119,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |119,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |120,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 59 |120,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |120,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |120,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |121,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |121,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 61 |121,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |121,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |121,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 63 |121,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |121,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |121,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |121,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |122,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |122,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 64 |122,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 65 |122,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |122,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 66 |122,37,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |122,45,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |122,46,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |122,46,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |124,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |124,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 63 |124,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |124,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |124,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |124,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 65 |124,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |124,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 66 |124,37,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |124,46,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 23 |124,64,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |124,64,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 23 |124,80,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |124,80,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 20.0 |124,81,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + div |124,85,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + sub |124,85,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gte |124,85,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + and |124,85,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_31 |124,87,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 46 |125,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |125,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 67 |125,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |125,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 33 |125,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |125,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |125,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |125,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 51 |127,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |127,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |127,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |127,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_32 |130,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_31 |130,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |131,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |131,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 68 |131,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 23 |131,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |131,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |131,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |131,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |132,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |132,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 22 |132,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |132,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 69 |132,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 22 |132,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |132,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |132,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 70 |133,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |133,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 23 |133,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |133,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |133,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |133,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_32 |134,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |135,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_33 + pushs 24 |137,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 71 |137,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |137,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |138,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 43 |138,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |138,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |138,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |139,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |139,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 72 |139,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |139,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |139,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 63 |139,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |139,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |139,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |139,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |140,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |140,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 63 |140,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |140,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |140,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |140,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |140,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |140,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 63 |140,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |140,40,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 3 |140,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |140,44,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + or |140,44,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_34 |140,45,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |141,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |141,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 22 |141,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |141,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 69 |141,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 21 |141,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |141,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |141,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 73 |142,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |142,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |142,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |142,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_35 |144,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_34 |144,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |145,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |145,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |145,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 48 |146,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushnil |146,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |146,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |147,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 57 |147,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |147,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |147,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_35 |148,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |149,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_36 + pushs 75 |153,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 76 |153,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |153,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |153,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |153,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |153,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |153,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |153,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |153,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 75 |155,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |155,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 77 |155,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |155,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |155,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |155,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |156,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 57 |156,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |156,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |156,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |157,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 58 |157,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |157,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |158,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_37 + pushs 62 |162,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |162,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |162,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |162,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 22 |162,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |162,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_38 |162,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |163,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |163,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 80 |163,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |163,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |163,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |164,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |164,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |164,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |164,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tput |164,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |165,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 59 |165,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |165,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |165,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |166,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 60 |166,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |166,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |167,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |167,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 22 |167,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |167,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 69 |167,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 22 |167,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |167,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |167,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_39 |168,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_38 |168,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |168,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |168,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |168,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |168,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 21 |168,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |168,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_40 |168,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |169,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |169,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 81 |169,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |169,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |169,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |170,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |170,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 69 |170,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 1 |170,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |170,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 28 |171,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |171,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 70 |171,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 1 |171,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |171,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 50 |172,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |172,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 67 |172,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |172,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tput |172,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 21 |173,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 32 |173,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |173,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |173,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 7 |174,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 59 |174,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |174,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 4 |175,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |175,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 5 |175,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |175,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 57 |175,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 21 |175,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |175,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |175,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jump @__label_32 |176,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_31 |176,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 50 |176,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |176,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 67 |176,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |176,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 16 |176,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - eq |176,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jumpz @__label_33 |176,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 50 |177,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |177,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 67 |177,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |177,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tput |177,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 21 |178,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 45 |178,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |178,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |178,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 71 |179,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |179,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |179,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |179,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jump @__label_34 |180,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_33 |180,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 50 |180,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |180,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 67 |180,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |180,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 400 |180,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - eq |180,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jumpz @__label_35 |180,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 50 |181,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |181,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 67 |181,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |181,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tput |181,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 72 |182,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |182,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |182,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |182,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 4 |183,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |183,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 5 |183,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |183,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 57 |183,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 400 |183,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |183,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |183,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jump @__label_36 |184,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_35 |184,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 50 |184,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |184,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 67 |184,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |184,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 401 |184,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - eq |184,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jumpz @__label_37 |184,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 50 |185,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |185,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 67 |185,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |185,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tput |185,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 73 |186,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |186,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |186,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |186,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 4 |187,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |187,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 5 |187,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |187,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 57 |187,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 401 |187,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |187,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |187,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_37 |189,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_36 |189,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_34 |189,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_32 |189,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_30 |189,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 4 |189,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |189,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 74 |189,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - tget |189,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 57 |189,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushl @__label_39 |190,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |203,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |203,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 21 |204,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |204,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |204,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |204,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 28 |205,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 82 |170,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |170,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |170,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |171,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |171,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |171,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |171,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tput |171,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |172,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 43 |172,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |172,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |172,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |173,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 71 |173,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |173,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |174,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |174,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 22 |174,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |174,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 69 |174,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 21 |174,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |174,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |174,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_41 |175,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_40 |175,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |175,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |175,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |175,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |175,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 16 |175,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |175,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_42 |175,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |176,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |176,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |176,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |176,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tput |176,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |177,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 57 |177,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |177,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |177,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 83 |178,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |178,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |178,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |178,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_43 |179,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_42 |179,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |179,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |179,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |179,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |179,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 400 |179,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |179,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_44 |179,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |180,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |180,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |180,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |180,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tput |180,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 84 |181,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |181,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |181,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |181,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |182,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |182,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 22 |182,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |182,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 69 |182,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 400 |182,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |182,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |182,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_45 |183,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_44 |183,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |183,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |183,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |183,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |183,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 401 |183,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |183,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_46 |183,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |184,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |184,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |184,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |184,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tput |184,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 85 |185,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |185,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |185,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |185,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |186,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |186,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 22 |186,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |186,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 69 |186,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 401 |186,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |186,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |186,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_46 |188,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_45 |188,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_43 |188,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_41 |188,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_39 |188,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |188,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |188,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 86 |188,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |188,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 69 |188,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushl @__label_48 |189,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |202,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |202,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |203,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |203,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |203,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |203,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |204,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |204,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 91 |204,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |204,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |204,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |204,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |204,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |205,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz gload |205,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 79 |205,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 7 |205,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |205,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |205,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |205,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 28 |206,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |206,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 80 |206,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 55 |206,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |206,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 2 |206,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |206,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - ret0 |207,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - -@__label_39 - pushs 75 |191,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |191,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 76 |191,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - lload 1 |191,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 77 |191,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - lload 2 |191,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 78 |191,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - lload 3 |191,59,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 6 |191,60,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |191,60,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - lload 2 |192,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 22 |192,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - eq |192,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 7 |192,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |192,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 46 |192,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - eq |192,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - and |192,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jumpz @__label_40 |192,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 21 |193,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 47 |193,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |193,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |193,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jump @__label_41 |194,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_40 |194,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - lload 2 |194,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 21 |194,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - eq |194,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jumpz @__label_42 |194,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 21 |195,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 32 |195,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |195,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gstore |195,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jump @__label_43 |196,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_42 |196,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - lload 2 |196,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 400 |196,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - eq |196,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 7 |196,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |196,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 46 |196,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - eq |196,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - and |196,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jumpz @__label_44 |196,44,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 72 |197,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |197,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |197,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |197,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jump @__label_45 |198,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_44 |198,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - lload 2 |198,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 401 |198,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - eq |198,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 7 |198,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |198,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 46 |198,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - eq |198,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - and |198,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - jumpz @__label_46 |198,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushs 73 |199,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - gload |199,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - pushi 0 |199,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - callc |199,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_46 |201,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_45 |201,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_43 |201,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_41 |201,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz - ret0 |201,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 92 |205,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 67 |205,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |205,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |205,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |205,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |206,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz @__label_48 - ret0 |211,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 87 |190,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |190,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 88 |190,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |190,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 89 |190,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |190,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 90 |190,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 3 |190,59,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 6 |190,60,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |190,60,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |191,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 22 |191,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |191,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |191,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |191,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 58 |191,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |191,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + and |191,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_49 |191,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |192,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 59 |192,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |192,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |192,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_50 |193,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_49 |193,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |193,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 21 |193,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |193,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_51 |193,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |194,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 43 |194,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |194,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |194,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_52 |195,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_51 |195,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |195,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 400 |195,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |195,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |195,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |195,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 58 |195,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |195,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + and |195,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_53 |195,44,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 84 |196,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |196,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |196,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |196,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_54 |197,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_53 |197,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |197,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 401 |197,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |197,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |197,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |197,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 58 |197,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |197,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + and |197,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_55 |197,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 85 |198,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |198,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |198,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |198,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_55 |200,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_54 |200,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_52 |200,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_50 |200,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |200,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz -@__label_49 - ret0 |215,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_57 + ret0 |210,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_58 + ret0 |214,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz diff --git a/script/testsolo.bdb b/script/testsolo.bdb index 877ba46ee655f8ac2f8f2be266af4c87c3e72cdf..d25ea48913a362e840bf458b3b0f8a48eb19684a 100644 GIT binary patch literal 65144 zcmb_kXPB1NwH*|}4pBj|z~BsnQ(yq4C>ErMjbcZy3Ns@J44^V#03#Y1%%y`0m;fde z9ZrgFeFeC?%4oP^XHpCd%e4?z0Y3n_d)f6 zIXU%n{&V`3FRsARYiqXSv9#cAIaMhTqn?~I9@92LxI49>4K`3$|O+Pdz=XOD8?m~L^ z@SL2-1Oe`aV~cJj61!0lPI4jbIWi|_yCC4taBMy`QG8?VV)-^+@vVt?5o`OV;#=d4 zw&ynZFH4TeXnP2_daT%VlTTabMk>~`+iXa$0I5)X>EZIJ?2w$CazQxWg;WcqVS;d& z3+W{E&x3+MhS~De7sHf3Wu2=}z~FlEqLC|}?hf|tQkCxM@}eB|sSpGtI2>D6w*#qC z5Pb8k2s3wrAhA9+pPDD*%&i!YTarZA4d9v;7kfH-pi43yF9cF2@!WS0{1ZH$Do8%|4FKh42$F9v zd|Gt`#(Fq5_Em$&j|hVAR=8O47@!WHj!``JAx&{8v`=7rn#93dF181Hp$9G%TTanf z+tXDKoGKW$+SUU#(x|LC=z%mU>+dLN8kO}p_(qjQRcg!Ao8a4a$u#~9$A&aj@hxlJ zkB0%@2xL`R6Jc8-o;Q`xSiZdjTfPzCO%`i=BnDy{l{FejtRf?@)!+cd`eMn24e1`R z{tiL#-60<;*8Ar5UJ&#J@rhH(7Tr3SQz`lOvlA3^$%a@4{msF?Mv8siU6rm<>^s4Q zv>Ql;f^dNg=|&KxLJ)kn;0_>F3X+eS8Ea;4W0XgIO# zTMyfTVgn+EWAox&n4c~PxCFw9^GeyH%qRA@|Yn=x$h3eNWenL0WSD(Y)DfTSADF%RB_d}F+T;Q zG)iig(mwzVC${ciDxUK$jcxU+Lz<6S zpGLKO3a(PC`}$`c$ndm85C9sE&8Ox{8}Tl+A>9R}t>TMM`Snw3hZ&D$^%|vi7*DM2 zvB^53MyVa2cKs;RL%IrEE#DUSzR&#@ z_dCC9#5f8kmO;mXLA)D$cWKXLY9ZMrk+m9E43{>7eu6_m9KQ2*m9@sMejAA{xYD1a_*2ArE*we8L+5!V=5EL#Pn->L+ z{&`1nrmxG3*HDy7LGV@j0TksfLGrP0zN!zB6i#e?2BJQUhj&PA)wi{GW=Sn_fZC?lLZxI{7gk#IxL15T4K|m56 zNW*|c*83P%2c$Htp9@D;N>oUhBf1|k?y0`X`q*l1SL|eFVr`4S&OGs)w^pp}OxTu) zZPr`k3)uD)+pIn}#|6#j(PtYw?*bAh1G8X5x=<-N-~l`a*lJZHGyJxgH>k~v50d5h6wrphttxO;YdVm&3IgwVTXc(5 zj@j?AIUbBVg!+e{HZLY4$IOTi>2a8MlFay&MH5ht8lOzEMK>Nu72;E_eAZI7$R1j@QzXQ@rLGW!E<>2u;LGsO@12GrR z5G39`v0U7t_Hy2yv9_hSxbx(>2qv6Z+izgY-5QrmCfj~US39xs&62~wuxUww3 z`PdJr-e_y9W`D(1-{e`2xjIR5fzZOS`SkBsziD{!0+5~&U$EFYe46X1bSqUY-s85+ zd;+8*iGy6VA+1o=^38*K$ng{66F|eU`P2p>Q4e%=AAZiT~7TXgf0 zNUftsu1L02|%2D4WQ5VyMyON7?D)7{8yj0`h5YUBJrF% zoDJzp^j>>G!UYqK4QVyZCkXH(i7Xo0JO-}v~L+feb%x1-Gm zMYc+wsFrMeTMoYQ2E)J&CzfwF!+`gwuM-zyURzb?4Pfr|;(68xUVts{N=9PKg1}ABhSUgGCf{PPOJgV2ZrCyo{>nbqwkt@+ZHnIx$J!p~ z;Q9X~Q#5P9ut`z_Mq;bMv5H~7_pN_~o4hdlA=eu(8Es%-?ffL%6)oT@x`R}D|NNW|3ebK$8*oQyEvH5g3*hgjVQ%YUH zzD?o_=bO!^Ral{UlYoff#IkQJ4ES_kW)5=jxDK{_I=!*wqP^;fLdk^<={waCzPDbz zk|~QLz`ivSm2bCOq?85w-j=7;ihWu4vs=MFI1-L;r(c3cQ>SOWwHttay~K;G5xk_B z%gJxc<2>-KwRpkZ#fG#6)Zn+yzFA&?W=x}$CZfHmN_-uD9=d6{BfKE50BQy@`u`PB7g2#M<2x0Bof z9^WI;`SSD#lCw&XeCoPN@tFS#(^k9RD;}4KZ#JYiFxff9vMR7{=mF}MtUd4XI|kDWSiDwdGB zv9{+bmSkmE87_|Y618t0d=9?-QjmP}U<1r236hU*CxIH%llZhBNL1~<3H-X^8&VdI zjk#xGMo~BUdSEZu2e5EBD`MH#2~9)gT_hN`czy&D5jgE^Nae{`y%AijmZ*F=eoryT zcgr0G22uO?xVT0s7IHDR?zbt<9ObI}J|J=S;LmVky*NxM7S<=$_En`=d=*&$=C&5k z`ToJi+@Dkr_%`th)dRd0Y(6~?q%IPj&!>xUYcCfhA9L3#=AuQziLLuv4!$i{s)U@4 zeeTUlm1O1WpA=WQAK0q75DY7njQOT)2}Z^MLE?sM^JzAa(kP{BB$lcKWDCdU(>R2* zP7s>Ake11Entz5Y{8I_(c2%P-3 zO5Y8ndj$zsTsStQClqIVdD;uk>=0jkv!c6F2i)y#(VYPH@z-_uGn`lkb;WH%bH6bXVaGed#IzVP+?P9UY((l@J4V~%Zk`dIac zZ`bS(N0v**eahiD)oDK6xU!xL;+5-i zpZgcZw*tw6t+q=P-pP@i!U0a`R1TP_Bv`gC(4or3yI7X%;s)~ovX-VwhGqU;o(P^xfjc{(2* z&To)>du&U^8Bidc*beWbIFp;vwh3;9USfk_!ijzExykgvNDSHz@^s&}Pz&>FLGpER zp<=EN>2n}06`xpJ8{bX>bE$HDH}8Jc13sTFP(8pm6}IS}g&yG6ne{E*-RK|c0i-(| zn-@)0M{r`rc0_m85j|X99Ey%876jiGvJ2+i#C@C5cGX+H34A4H`ErTQ$K&RzBYYEh z6WDjM_{3Www(cLnfN?O#v9@c_ew~fay$xLD@ASAo$3Ax-Y}qLV@_bvz)F?f`{elr_SvT_SM7vGftkzyWW?Vjdq&dkd{fyE#)c7`^3UC`yf08q?^|=%U&6A9@l+Re(iKy*BvB}Dd(V!$1 zfNwSqQMK*ws%=|1GF=dSZ{P0$(p*8}W^1eM*FakOvmD!yK2!=IR}gJTeU$>>24zEP zt(c24h7-%aX|Ux~@NK6jfXBV$=|^YuqIWU{&nfO^DoC6Mw)*S_QW`C> z1Ldt2pM2AQmSes z$&s1QfMnHN=c_L5>GGnn>QY~C?NPnO4B6OstTje#XjHfKBp+{!?zo@j(AdO zA0N_2P>yeFvTi8vfk8-4IKC}#wPH|#Alfo>HtK$L5}%eNV^9MmHjU2Pf^@Hz=yF|o zx(apA69j(7PL4^aJ9SZ3&b$v>Y7g$Bwo0Fa`jklo_%j?E(o$7vYD61S!(^4N1QHd! zPxXD8Oc#~lwxJH^)QRQH0L3?Uc&u$MIMYii%v&MWcAU}*7@7{29E^&L7X&22fiyzZ z_7WG;%Rs6Ygj^TWX;=lOCqcR&NK_qgFC1Gp{Q#t;fnp!D3d0P2XY zrGwRdlI{CV)V}YF=S{Wm`zCuw#YJDW3K7y;sS@0CRO<>1kIjPMLwZr|`@FYer`gwF zA2lMs!;iH+*}=EIifkM}0yTRFC>6%rM$sc>x3%>>`p3j)74wIOXp za%u&M^Tvj>L-CFCIF>WtD86xPi?!W^R8m=>p~H!_%>!4@6C1dn$#zXLEwK*_J5!!s zPd;rmcs|+QJ_`)nAie-J9Gg%1$Z>B$V05wd`Cr&l0c5T1$FL=rnzMC;Ja~psoHXq+VXe=%+qLzTYyBJc!gxf=F^{nR4oX(E~G^m3278S zJur8v_~KIlUtw(Uc63<2QX^ZCs6uVduI6-VkcL3=$K|-s9V?(M+ z*0WtvqUqubqqO;SBl=^7AhB9Dq*B!%xcI}dA#GAx&i6}>mz9txm2RJ)wUYc=qL!jT)d%r!1pWBZb-*;@u|cW-7?ipzHej>R^5aF z6pk(9ryer-Lxz z*z&Yp@r~2ThIF8VGdmodxfk04HGWpUZAzw%J_2*Ovw(VzioB{=;=7p_fF&En7m(QD zQ+-EmcRFf&pQEe=JB;(B!HOX)Tb3N>skP>p0Lgjn+F0 z*9^5d+zTgm@-K$%F=E5*$d;Ldz_(_Cgg?WH^`Z_2U5vKvm1;o?XL{~j*bWjK-g~h= zPr>9#qf;(X?DQ$@W{Oj+g{_)z18K5k#;2WDU2P8hROw*ud@y&oJdYb&>~jYuW9}VbNgD0c8$rzYSvjtyytQanX2 zq&E~x=u>R9M!+Cno}P6^4ML@P7t+?2i_b7ws9Ku1a&f2PjPG{cgGy5+^IexMx<6pD zq|rY+lW}Gw>Ruw5hI`?}a^_!{4rvrk85q>^XI|KHTni-L&pvIm0mFZ&_~g4$URMnA z?Lvzcg8&+iEl+idLA+UPNQZ(!+^MtP5xZbZeS?M#CzgGkz&_3j1e3}3N5$N>Vrz?j zyW(3{7t)pBTbUsEcJGbJnA;ph881Gya7DKo)JUUtjso$QCCO6*(56n1e0|viOBvrZ z@Eut!k85DtTfFdXiJetPWc8vrnL3!2OdVVW9+yjII9Y66`bQuQ6D0f@jt!}W>WEwy z()&P~EC@dB^Juc(IvTz8p!np=(>b`Q(x|A0s!M&})KvoMIf;(_VPnBUblPx1;yoPO z?S)B<4~f91py03IJS&; zQ>^wy_qNh%?6g=0eTBMn^YN*q$*4P(wNEAe9Y|9oxA@bM<35T(>@8ch#;dxcVI4lb zqpHQ|VyiV8lq->^gFfNJ+77{XnMN0VtvKWRmHgd`GgSSy`s_u0sI2+#IBZA{A+c1} zzK+<1J7T+3$;ZA%s5^BL_xaemS1HcGp-i0F4O?m|&ZyYu9tgfs(YKT5+A4CRVlE@F zA?*NCq4;u+%cp&cZ@!JIM)A$pP3L3#r|RR)8(XcFN;!~Wwj7_SlmmZn8S6!T#Tnll zmz}E8-CUKv7FQ~j181%+Gn;`lOnl+ww;|=joLV~972R>*@q>ck+hY$$Yt{*pZ@0b~ zNY4q9ZrIaoOUTrx@h> z{pLTwpef>uZ!@}7ZTP-z?J31Ytmxs`@-z`4JuC>mw{Yiy)lUhMkJU$li_a!OdSB_6 ptX!P07?da8*mC?mkf>XI`~Ea=k-F8VK`v8V^!590R4a{g_^w55&aZVKn1j*5=1^F34Zw^0bEcPMH3ZJl%;?u6_b#VU`RAy5QvB691@O# z%Ax|b;6jicTtFdugeZ$kP_Zr0R%`{UP%GAoRzT6-^LD0Zem{4fKjcsDnYnXk=DyGS z=1|il7W-A~f9GuZ_lj6fNyYf0oYDvK%X13ztL`l=A3ZUZld3GtsjNs9OsKBTNflO> z-dB}VRg|hqRg_h9DX6YK45ae~p(lm(;%Tv1LJ;~=NM&clVl{%0P9ZI77K_yh!kHA( z_U5tJ20=KBLi(mfEY=_h0F6#q?bsCVhZUb7+)y}aSCY%$k`|e z0F6!@J!T4cQ-=>`qUE@_n6p2}{7QD!!UGzdFb=l8i3(n_dcf*_niA)RGZz-{wC z0_hQnr2`ey7^vU{K>+#D2~)vbs9={MAUp!;VK}iig1|9;#CW>Cf*6+FV529lTV4TD zLSo6KVp;`dP7(xe7Tk}5)d&)&%soJQ)Fq}Rm~?f5#7*CMMwyU%bi!2dJ&+m%f$Nr8 zP-Z_tYAOGQ#q_GtQyzs>iQ3h=WcReG9oH@Ip`tC=g@iYo)lBpfn!OINe`aU5nyJlc~m1M>F%-yDT+-*oL97ZpR3FJp7EW73= zJ1(YtHz#%pvP()oiIK9 z(=g8IX@hY++{`>5Q$@XR7b?446imGe_f%GazEkhZW%q>f1)MUE8DEe~@ifZ#0`6^6 zgYg9eshBoGPm=`!f{0F-#xF2>>PsPg3M6&WT({h3^u#@FEOzq+F$|>!;fM1DPePfa z1&Qms<8B(Cg4)$eOk4*aFe>1XR$)FZlbE;VqZB)SB`1N!1biIuVAg$0-crI=FB&q zt`n8r4mVGiFv^4=q7%k=J$_x?A$P0Y0driRHu_RA^>av(U+^Cw-6<;Iw$z<&dfMXV#Hyi8bv<#a@18NPhnr7LjGnmn!DHR@ z)Ci=6@WVN=AE2iKK?3>F37fum89iy&gduHp(^Dt-0(JJB&s^c=#GZvR?-8a!essck zDuD{r%W<<{mz$m%P`hf$3ZO)G>85srDWr#hR4WK^3h65#>Fqy(kbGRvJ=k|8E{sn{N-qi$H#46%4g=vuCv3MgH!9$!??*t& zl2|wwebUqpawoFe;pSBm#;XjYVtUlgs|o_`LJiP{_=LG>j5=dWQ zX8Jg>^-$&q5(}h7#B{FlbeuAiZhAUuRFI^y>j=hI3j)_IhfMA8V|2pwG|{+2&S7M` zF&;NOVZadC-3n#;c)Cf3C%rcaWBeMFUR^ZD_!1zgr$cxIPeq`)L=ZTKahXxU2r8yp zs6ag(_XIT1sDR6EwQ+|?G&*4_Xo*3o&j{SR*S$c>l32LxdO}a#1PPQyCoH>Dp(lMt z;C67aQ6{H?R)+C(D!UYrG6jL#Qu|EpICt2@FwVs^7D%O%9jAguAnARkew!7hr^nH3 zGbJWYPj?w7#_4IRn-kjrntlA!`B(#bC&sCuA10mpOK#Gw1(I$j&Z|_p_1zOjPuy1l z$AF~I2-(zB8D*5o-5NC*Wpa#vg%n=N8V>O*OSkr?R`w%^lVOX^6yvM57a?g4u>~Ze|WeAg@a-+$?B=siMxF zV|+i5vLq(%`|-Y}cAOJy22ZEXp4$jzKq{5&xJkDgNIpJupm9Ci%zVks^|Up5N(f(J z-SUai6Zg#7+^B$?bRU`8wWG58E#~-4LEt=HzHwq)-|a9?jO&(vW74S;;~1~Uq|>L3 zG>Y+;jmEh-UJYe-mRPuL{*+Mx_kCs-7+)K{sd^(n{l1+EOuHZ5ZdK%3ScIK9k$#cc5*3 zTu+S~<2Sl_mHV+i@X$!i=$Eys5#?i#l(SO4>!Y1Q-xbJ$V z8P~(T3H;1-3+JM5Hr>Kax>itTp)ih76`e4~Q$QLg2wd&xo)2BOrrv>Tjqdf zA1Aia@RTLYhw+qWc;cMcV(955i3OBJCoHBh(35&!&X3PEjJKp>`VHC^2}XzOmJCcf z)i&o22OCcZ%Aym-csp#)LP6ks<`f`}6C{qOJrL1yLE@a)-Kd>bpPQMPrgpg!TNvYC z11Tv8T(_iw@iBtL?YpT!DszeHAdq~V*j_gkbb%bVxn#EudQxp?3SyWFW)z47a7B|CsdCk*K{qbF{T z?=pJgkm}uhW`C4kzYyYl=5t`&$7hy;C-s@!-@reKq4c&?U!6vn3Wgg`$4$D|Ozo1C zG6$e`(*=S1@@1*19oKgYK+d}o6Gl~Z!Wb{a%+#83`!0z<)Y<1!F&zbx){L7fBcKBP ze}Fi~JHla%k*v6>G6P6ug2d_Ra;RXNAaQrm3xV_pLE>)LvW*Jz1TjpR-x(EfwTqkD zaXcM1J~N%lZa;**!libLP`iCDwR_jiKg~n!5)xAe#nVfezOw~^>$|pKT>Vp=ifJ;C z)Y)^&ydJ;4R$}2MU324~xZB!~jDOkcFlz?${Vq6E0 zGOkAv!!+LDR=bDXT+bk*0`800tKIxlDQc%)j#EK5OgeSZdDMJ54y2?YaIWV~s9>ld zadSMwI5Dnv4;wvk9ee~xdRv=LF@6BMWrZMcoAVksWp+1u8bD=t!sv;cDsMqggCrKN zc9}*`+_xcL8x>?wJiUtfH1Wq7%mWG$5&CRW}`mbTd|yK5cM)*9s}=?LT)Hz1#Rq z?yoHt85MM<7~c#PJR%4HjZPTjYr(ksOm5#*fpH(7nUCqKcVgTe9|oSL3qM?T>%h~? zf`lKV6UO)i!xPtc<;K%-Gqb>r@yD>G%7h<|@!_c53_${Dbix?#ffiB6dJBd03y_k6 zz&&#og7GT^iBrMbK=Sb_Nzgn-V&c5Y95)pl1(JH-%c$BN!j@Vo2wb;(jDo4dNT*`D z4uNbJ1a9BOpaNAUx24`OdgA<3w$W3Z;%P3B#tQ=XeD|HvQx__xxSM~faq~}yjm9}A zcB^q>+#GL%Zc!(8F~#^EHz&3Pb37rja8qR-^i(NG_%S+RTj~urPj|6VCZ~d0H)Rfl zp4JIJoPXMDRKR_c^|Voe?&7f8Z85dO_#{*?6O2DC2%Il?88oY7)tkSt?5YunI(u$Q zrHo_c-XrWYD#)Z5zYDdi5d^Mw4;r4hnOTMG^jinctMrD()%y;icq#!oR|>+}6w;^Q zN&OR7yFBBcl&>(2_c1)_O>!7gQ}CotjGIsI14*6zB^2Y`u$smQLYzW64kY#CoPRoE zybAX=sU7s><1o4#o;b!A0I5=#<~sNUkfyjGWxDx-uTAZ^IX=Yr0&YvCK?R$ICxjQB zunw-nEJ*wxfkE14d;zEN&PZvV!~{V^CoH?!KvIXnxx-Cv4r9OB=Gjzs%S`RKzuvjW z%@=HT<7qlHzD4)}Wzh*!!DQnwxNSb(%@^bw#yO4GgYgZ*4=5ve`W(NmUZod>)Zcg& zZf4%#=2co4uaZS&S8i0meLwzt<9a$#F%2+`bK5-Ejqx53c9CSoP2b-E$;V;*0wf=Y zan$&N4pi+%po8_kfb#_%(ZT8qxMzess6eygZXp-D)h>paskP(u^p2ZX$%YE%OZ5R7 zov42M8xeZmD)4cKUm_57hrOworWqA*%53H4=~8Z(iA^izRg9QQ7icVO!^u|Ug5`>l%(lK=KNQ~O1>?n%6}MZK0x2a(+#EmM z)QMDp~+1+CF#7(*vfRvC}uA^eQ m*UdkDXjGt2v0*$d!~pPdVzZ%wUBVB-i%wWf{{oVa6ZErMjbcZy3Ns@J44^V#03#Y1%%y`0m;fde z9ZrgFeFeC?%4oP^XHpCd%e4?z0Y3n_d)f6 zIXU%n{&V`3FRsARYiqXSv9#cAIaMhTqn?~I9@92LxI49>4K`3$|O+Pdz=XOD8?m~L^ z@SL2-1Oe`aV~cJj61!0lPI4jbIWi|_yCC4taBMy`QG8?VV)-^+@vVt?5o`OV;#=d4 zw&ynZFH4TeXnP2_daT%VlTTabMk>~`+iXa$0I5)X>EZIJ?2w$CazQxWg;WcqVS;d& z3+W{E&x3+MhS~De7sHf3Wu2=}z~FlEqLC|}?hf|tQkCxM@}eB|sSpGtI2>D6w*#qC z5Pb8k2s3wrAhA9+pPDD*%&i!YTarZA4d9v;7kfH-pi43yF9cF2@!WS0{1ZH$Do8%|4FKh42$F9v zd|Gt`#(Fq5_Em$&j|hVAR=8O47@!WHj!``JAx&{8v`=7rn#93dF181Hp$9G%TTanf z+tXDKoGKW$+SUU#(x|LC=z%mU>+dLN8kO}p_(qjQRcg!Ao8a4a$u#~9$A&aj@hxlJ zkB0%@2xL`R6Jc8-o;Q`xSiZdjTfPzCO%`i=BnDy{l{FejtRf?@)!+cd`eMn24e1`R z{tiL#-60<;*8Ar5UJ&#J@rhH(7Tr3SQz`lOvlA3^$%a@4{msF?Mv8siU6rm<>^s4Q zv>Ql;f^dNg=|&KxLJ)kn;0_>F3X+eS8Ea;4W0XgIO# zTMyfTVgn+EWAox&n4c~PxCFw9^GeyH%qRA@|Yn=x$h3eNWenL0WSD(Y)DfTSADF%RB_d}F+T;Q zG)iig(mwzVC${ciDxUK$jcxU+Lz<6S zpGLKO3a(PC`}$`c$ndm85C9sE&8Ox{8}Tl+A>9R}t>TMM`Snw3hZ&D$^%|vi7*DM2 zvB^53MyVa2cKs;RL%IrEE#DUSzR&#@ z_dCC9#5f8kmO;mXLA)D$cWKXLY9ZMrk+m9E43{>7eu6_m9KQ2*m9@sMejAA{xYD1a_*2ArE*we8L+5!V=5EL#Pn->L+ z{&`1nrmxG3*HDy7LGV@j0TksfLGrP0zN!zB6i#e?2BJQUhj&PA)wi{GW=Sn_fZC?lLZxI{7gk#IxL15T4K|m56 zNW*|c*83P%2c$Htp9@D;N>oUhBf1|k?y0`X`q*l1SL|eFVr`4S&OGs)w^pp}OxTu) zZPr`k3)uD)+pIn}#|6#j(PtYw?*bAh1G8X5x=<-N-~l`a*lJZHGyJxgH>k~v50d5h6wrphttxO;YdVm&3IgwVTXc(5 zj@j?AIUbBVg!+e{HZLY4$IOTi>2a8MlFay&MH5ht8lOzEMK>Nu72;E_eAZI7$R1j@QzXQ@rLGW!E<>2u;LGsO@12GrR z5G39`v0U7t_Hy2yv9_hSxbx(>2qv6Z+izgY-5QrmCfj~US39xs&62~wuxUww3 z`PdJr-e_y9W`D(1-{e`2xjIR5fzZOS`SkBsziD{!0+5~&U$EFYe46X1bSqUY-s85+ zd;+8*iGy6VA+1o=^38*K$ng{66F|eU`P2p>Q4e%=AAZiT~7TXgf0 zNUftsu1L02|%2D4WQ5VyMyON7?D)7{8yj0`h5YUBJrF% zoDJzp^j>>G!UYqK4QVyZCkXH(i7Xo0JO-}v~L+feb%x1-Gm zMYc+wsFrMeTMoYQ2E)J&CzfwF!+`gwuM-zyURzb?4Pfr|;(68xUVts{N=9PKg1}ABhSUgGCf{PPOJgV2ZrCyo{>nbqwkt@+ZHnIx$J!p~ z;Q9X~Q#5P9ut`z_Mq;bMv5H~7_pN_~o4hdlA=eu(8Es%-?ffL%6)oT@x`R}D|NNW|3ebK$8*oQyEvH5g3*hgjVQ%YUH zzD?o_=bO!^Ral{UlYoff#IkQJ4ES_kW)5=jxDK{_I=!*wqP^;fLdk^<={waCzPDbz zk|~QLz`ivSm2bCOq?85w-j=7;ihWu4vs=MFI1-L;r(c3cQ>SOWwHttay~K;G5xk_B z%gJxc<2>-KwRpkZ#fG#6)Zn+yzFA&?W=x}$CZfHmN_-uD9=d6{BfKE50BQy@`u`PB7g2#M<2x0Bof z9^WI;`SSD#lCw&XeCoPN@tFS#(^k9RD;}4KZ#JYiFxff9vMR7{=mF}MtUd4XI|kDWSiDwdGB zv9{+bmSkmE87_|Y618t0d=9?-QjmP}U<1r236hU*CxIH%llZhBNL1~<3H-X^8&VdI zjk#xGMo~BUdSEZu2e5EBD`MH#2~9)gT_hN`czy&D5jgE^Nae{`y%AijmZ*F=eoryT zcgr0G22uO?xVT0s7IHDR?zbt<9ObI}J|J=S;LmVky*NxM7S<=$_En`=d=*&$=C&5k z`ToJi+@Dkr_%`th)dRd0Y(6~?q%IPj&!>xUYcCfhA9L3#=AuQziLLuv4!$i{s)U@4 zeeTUlm1O1WpA=WQAK0q75DY7njQOT)2}Z^MLE?sM^JzAa(kP{BB$lcKWDCdU(>R2* zP7s>Ake11Entz5Y{8I_(c2%P-3 zO5Y8ndj$zsTsStQClqIVdD;uk>=0jkv!c6F2i)y#(VYPH@z-_uGn`lkb;WH%bH6bXVaGed#IzVP+?P9UY((l@J4V~%Zk`dIac zZ`bS(N0v**eahiD)oDK6xU!xL;+5-i zpZgcZw*tw6t+q=P-pP@i!U0a`R1TP_Bv`gC(4or3yI7X%;s)~ovX-VwhGqU;o(P^xfjc{(2* z&To)>du&U^8Bidc*beWbIFp;vwh3;9USfk_!ijzExykgvNDSHz@^s&}Pz&>FLGpER zp<=EN>2n}06`xpJ8{bX>bE$HDH}8Jc13sTFP(8pm6}IS}g&yG6ne{E*-RK|c0i-(| zn-@)0M{r`rc0_m85j|X99Ey%876jiGvJ2+i#C@C5cGX+H34A4H`ErTQ$K&RzBYYEh z6WDjM_{3Www(cLnfN?O#v9@c_ew~fay$xLD@ASAo$3Ax-Y}qLV@_bvz)F?f`{elr_SvT_SM7vGftkzyWW?Vjdq&dkd{fyE#)c7`^3UC`yf08q?^|=%U&6A9@l+Re(iKy*BvB}Dd(V!$1 zfNwSqQMK*ws%=|1GF=dSZ{P0$(p*8}W^1eM*FakOvmD!yK2!=IR}gJTeU$>>24zEP zt(c24h7-%aX|Ux~@NK6jfXBV$=|^YuqIWU{&nfO^DoC6Mw)*S_QW`C> z1Ldt2pM2AQmSes z$&s1QfMnHN=c_L5>GGnn>QY~C?NPnO4B6OstTje#XjHfKBp+{!?zo@j(AdO zA0N_2P>yeFvTi8vfk8-4IKC}#wPH|#Alfo>HtK$L5}%eNV^9MmHjU2Pf^@Hz=yF|o zx(apA69j(7PL4^aJ9SZ3&b$v>Y7g$Bwo0Fa`jklo_%j?E(o$7vYD61S!(^4N1QHd! zPxXD8Oc#~lwxJH^)QRQH0L3?Uc&u$MIMYii%v&MWcAU}*7@7{29E^&L7X&22fiyzZ z_7WG;%Rs6Ygj^TWX;=lOCqcR&NK_qgFC1Gp{Q#t;fnp!D3d0P2XY zrGwRdlI{CV)V}YF=S{Wm`zCuw#YJDW3K7y;sS@0CRO<>1kIjPMLwZr|`@FYer`gwF zA2lMs!;iH+*}=EIifkM}0yTRFC>6%rM$sc>x3%>>`p3j)74wIOXp za%u&M^Tvj>L-CFCIF>WtD86xPi?!W^R8m=>p~H!_%>!4@6C1dn$#zXLEwK*_J5!!s zPd;rmcs|+QJ_`)nAie-J9Gg%1$Z>B$V05wd`Cr&l0c5T1$FL=rnzMC;Ja~psoHXq+VXe=%+qLzTYyBJc!gxf=F^{nR4oX(E~G^m3278S zJur8v_~KIlUtw(Uc63<2QX^ZCs6uVduI6-VkcL3=$K|-s9V?(M+ z*0WtvqUqubqqO;SBl=^7AhB9Dq*B!%xcI}dA#GAx&i6}>mz9txm2RJ)wUYc=qL!jT)d%r!1pWBZb-*;@u|cW-7?ipzHej>R^5aF z6pk(9ryer-Lxz z*z&Yp@r~2ThIF8VGdmodxfk04HGWpUZAzw%J_2*Ovw(VzioB{=;=7p_fF&En7m(QD zQ+-EmcRFf&pQEe=JB;(B!HOX)Tb3N>skP>p0Lgjn+F0 z*9^5d+zTgm@-K$%F=E5*$d;Ldz_(_Cgg?WH^`Z_2U5vKvm1;o?XL{~j*bWjK-g~h= zPr>9#qf;(X?DQ$@W{Oj+g{_)z18K5k#;2WDU2P8hROw*ud@y&oJdYb&>~jYuW9}VbNgD0c8$rzYSvjtyytQanX2 zq&E~x=u>R9M!+Cno}P6^4ML@P7t+?2i_b7ws9Ku1a&f2PjPG{cgGy5+^IexMx<6pD zq|rY+lW}Gw>Ruw5hI`?}a^_!{4rvrk85q>^XI|KHTni-L&pvIm0mFZ&_~g4$URMnA z?Lvzcg8&+iEl+idLA+UPNQZ(!+^MtP5xZbZeS?M#CzgGkz&_3j1e3}3N5$N>Vrz?j zyW(3{7t)pBTbUsEcJGbJnA;ph881Gya7DKo)JUUtjso$QCCO6*(56n1e0|viOBvrZ z@Eut!k85DtTfFdXiJetPWc8vrnL3!2OdVVW9+yjII9Y66`bQuQ6D0f@jt!}W>WEwy z()&P~EC@dB^Juc(IvTz8p!np=(>b`Q(x|A0s!M&})KvoMIf;(_VPnBUblPx1;yoPO z?S)B<4~f91py03IJS&; zQ>^wy_qNh%?6g=0eTBMn^YN*q$*4P(wNEAe9Y|9oxA@bM<35T(>@8ch#;dxcVI4lb zqpHQ|VyiV8lq->^gFfNJ+77{XnMN0VtvKWRmHgd`GgSSy`s_u0sI2+#IBZA{A+c1} zzK+<1J7T+3$;ZA%s5^BL_xaemS1HcGp-i0F4O?m|&ZyYu9tgfs(YKT5+A4CRVlE@F zA?*NCq4;u+%cp&cZ@!JIM)A$pP3L3#r|RR)8(XcFN;!~Wwj7_SlmmZn8S6!T#Tnll zmz}E8-CUKv7FQ~j181%+Gn;`lOnl+ww;|=joLV~972R>*@q>ck+hY$$Yt{*pZ@0b~ zNY4q9ZrIaoOUTrx@h> z{pLTwpef>uZ!@}7ZTP-z?J31Ytmxs`@-z`4JuC>mw{Yiy)lUhMkJU$li_a!OdSB_6 ptX!P07?da8*mC?mkf>XI`~Ea=k-F8VK`v8V^!590R4a{g_^w55&aZVKn1j*5=1^F34Zw^0bEcPMH3ZJl%;?u6_b#VU`RAy5QvB691@O# z%Ax|b;6jicTtFdugeZ$kP_Zr0R%`{UP%GAoRzT6-^LD0Zem{4fKjcsDnYnXk=DyGS z=1|il7W-A~f9GuZ_lj6fNyYf0oYDvK%X13ztL`l=A3ZUZld3GtsjNs9OsKBTNflO> z-dB}VRg|hqRg_h9DX6YK45ae~p(lm(;%Tv1LJ;~=NM&clVl{%0P9ZI77K_yh!kHA( z_U5tJ20=KBLi(mfEY=_h0F6#q?bsCVhZUb7+)y}aSCY%$k`|e z0F6!@J!T4cQ-=>`qUE@_n6p2}{7QD!!UGzdFb=l8i3(n_dcf*_niA)RGZz-{wC z0_hQnr2`ey7^vU{K>+#D2~)vbs9={MAUp!;VK}iig1|9;#CW>Cf*6+FV529lTV4TD zLSo6KVp;`dP7(xe7Tk}5)d&)&%soJQ)Fq}Rm~?f5#7*CMMwyU%bi!2dJ&+m%f$Nr8 zP-Z_tYAOGQ#q_GtQyzs>iQ3h=WcReG9oH@Ip`tC=g@iYo)lBpfn!OINe`aU5nyJlc~m1M>F%-yDT+-*oL97ZpR3FJp7EW73= zJ1(YtHz#%pvP()oiIK9 z(=g8IX@hY++{`>5Q$@XR7b?446imGe_f%GazEkhZW%q>f1)MUE8DEe~@ifZ#0`6^6 zgYg9eshBoGPm=`!f{0F-#xF2>>PsPg3M6&WT({h3^u#@FEOzq+F$|>!;fM1DPePfa z1&Qms<8B(Cg4)$eOk4*aFe>1XR$)FZlbE;VqZB)SB`1N!1biIuVAg$0-crI=FB&q zt`n8r4mVGiFv^4=q7%k=J$_x?A$P0Y0driRHu_RA^>av(U+^Cw-6<;Iw$z<&dfMXV#Hyi8bv<#a@18NPhnr7LjGnmn!DHR@ z)Ci=6@WVN=AE2iKK?3>F37fum89iy&gduHp(^Dt-0(JJB&s^c=#GZvR?-8a!essck zDuD{r%W<<{mz$m%P`hf$3ZO)G>85srDWr#hR4WK^3h65#>Fqy(kbGRvJ=k|8E{sn{N-qi$H#46%4g=vuCv3MgH!9$!??*t& zl2|wwebUqpawoFe;pSBm#;XjYVtUlgs|o_`LJiP{_=LG>j5=dWQ zX8Jg>^-$&q5(}h7#B{FlbeuAiZhAUuRFI^y>j=hI3j)_IhfMA8V|2pwG|{+2&S7M` zF&;NOVZadC-3n#;c)Cf3C%rcaWBeMFUR^ZD_!1zgr$cxIPeq`)L=ZTKahXxU2r8yp zs6ag(_XIT1sDR6EwQ+|?G&*4_Xo*3o&j{SR*S$c>l32LxdO}a#1PPQyCoH>Dp(lMt z;C67aQ6{H?R)+C(D!UYrG6jL#Qu|EpICt2@FwVs^7D%O%9jAguAnARkew!7hr^nH3 zGbJWYPj?w7#_4IRn-kjrntlA!`B(#bC&sCuA10mpOK#Gw1(I$j&Z|_p_1zOjPuy1l z$AF~I2-(zB8D*5o-5NC*Wpa#vg%n=N8V>O*OSkr?R`w%^lVOX^6yvM57a?g4u>~Ze|WeAg@a-+$?B=siMxF zV|+i5vLq(%`|-Y}cAOJy22ZEXp4$jzKq{5&xJkDgNIpJupm9Ci%zVks^|Up5N(f(J z-SUai6Zg#7+^B$?bRU`8wWG58E#~-4LEt=HzHwq)-|a9?jO&(vW74S;;~1~Uq|>L3 zG>Y+;jmEh-UJYe-mRPuL{*+Mx_kCs-7+)K{sd^(n{l1+EOuHZ5ZdK%3ScIK9k$#cc5*3 zTu+S~<2Sl_mHV+i@X$!i=$Eys5#?i#l(SO4>!Y1Q-xbJ$V z8P~(T3H;1-3+JM5Hr>Kax>itTp)ih76`e4~Q$QLg2wd&xo)2BOrrv>Tjqdf zA1Aia@RTLYhw+qWc;cMcV(955i3OBJCoHBh(35&!&X3PEjJKp>`VHC^2}XzOmJCcf z)i&o22OCcZ%Aym-csp#)LP6ks<`f`}6C{qOJrL1yLE@a)-Kd>bpPQMPrgpg!TNvYC z11Tv8T(_iw@iBtL?YpT!DszeHAdq~V*j_gkbb%bVxn#EudQxp?3SyWFW)z47a7B|CsdCk*K{qbF{T z?=pJgkm}uhW`C4kzYyYl=5t`&$7hy;C-s@!-@reKq4c&?U!6vn3Wgg`$4$D|Ozo1C zG6$e`(*=S1@@1*19oKgYK+d}o6Gl~Z!Wb{a%+#83`!0z<)Y<1!F&zbx){L7fBcKBP ze}Fi~JHla%k*v6>G6P6ug2d_Ra;RXNAaQrm3xV_pLE>)LvW*Jz1TjpR-x(EfwTqkD zaXcM1J~N%lZa;**!libLP`iCDwR_jiKg~n!5)xAe#nVfezOw~^>$|pKT>Vp=ifJ;C z)Y)^&ydJ;4R$}2MU324~xZB!~jDOkcFlz?${Vq6E0 zGOkAv!!+LDR=bDXT+bk*0`800tKIxlDQc%)j#EK5OgeSZdDMJ54y2?YaIWV~s9>ld zadSMwI5Dnv4;wvk9ee~xdRv=LF@6BMWrZMcoAVksWp+1u8bD=t!sv;cDsMqggCrKN zc9}*`+_xcL8x>?wJiUtfH1Wq7%mWG$5&CRW}`mbTd|yK5cM)*9s}=?LT)Hz1#Rq z?yoHt85MM<7~c#PJR%4HjZPTjYr(ksOm5#*fpH(7nUCqKcVgTe9|oSL3qM?T>%h~? zf`lKV6UO)i!xPtc<;K%-Gqb>r@yD>G%7h<|@!_c53_${Dbix?#ffiB6dJBd03y_k6 zz&&#og7GT^iBrMbK=Sb_Nzgn-V&c5Y95)pl1(JH-%c$BN!j@Vo2wb;(jDo4dNT*`D z4uNbJ1a9BOpaNAUx24`OdgA<3w$W3Z;%P3B#tQ=XeD|HvQx__xxSM~faq~}yjm9}A zcB^q>+#GL%Zc!(8F~#^EHz&3Pb37rja8qR-^i(NG_%S+RTj~urPj|6VCZ~d0H)Rfl zp4JIJoPXMDRKR_c^|Voe?&7f8Z85dO_#{*?6O2DC2%Il?88oY7)tkSt?5YunI(u$Q zrHo_c-XrWYD#)Z5zYDdi5d^Mw4;r4hnOTMG^jinctMrD()%y;icq#!oR|>+}6w;^Q zN&OR7yFBBcl&>(2_c1)_O>!7gQ}CotjGIsI14*6zB^2Y`u$smQLYzW64kY#CoPRoE zybAX=sU7s><1o4#o;b!A0I5=#<~sNUkfyjGWxDx-uTAZ^IX=Yr0&YvCK?R$ICxjQB zunw-nEJ*wxfkE14d;zEN&PZvV!~{V^CoH?!KvIXnxx-Cv4r9OB=Gjzs%S`RKzuvjW z%@=HT<7qlHzD4)}Wzh*!!DQnwxNSb(%@^bw#yO4GgYgZ*4=5ve`W(NmUZod>)Zcg& zZf4%#=2co4uaZS&S8i0meLwzt<9a$#F%2+`bK5-Ejqx53c9CSoP2b-E$;V;*0wf=Y zan$&N4pi+%po8_kfb#_%(ZT8qxMzess6eygZXp-D)h>paskP(u^p2ZX$%YE%OZ5R7 zov42M8xeZmD)4cKUm_57hrOworWqA*%53H4=~8Z(iA^izRg9QQ7icVO!^u|Ug5`>l%(lK=KNQ~O1>?n%6}MZK0x2a(+#EmM z)QMDp~+1+CF#7(*vfRvC}uA^eQ m*UdkDXjGt2v0*$d!~pPdVzZ%wUBVB-i%wWf{{oVa6ZZREp1b*4?UucmDP5v6_uS^^CvQd&$?z|Wj%G6{hsQzw%`S-H^T zQq*&F>pA`n9`+uO#|?jgE`*gIOBe3_-TPjWnWo?(Z+>&X-o3wj-*qk!!6HJ z%e_grXp^7oRQB4`uD7V+wrVhFf9wb3v}z5PoWNa16s zz^#~P$tmCH!j{)nmRfGTb`9HLUGw}lt@)l)DLd_eirKm_0Z_F& zO}ggZa%x_SpbgwAozGp(E+9(NStsAEbl_0-I<0^jUX6ObV;RYU^$rcLvT%mM%rI=r!HOKesu3u`qK@Z<_>b)0iUavJ>zqUzb z-$fohE4)=UX}uFry+Sp(o4~v|x^dfu97N~m7jqOiH(ak;1s@l3!VBJ)t5qaiZGhN7 zO6;_$;5M9~zTv8MuU!x7UJDjoNKPZr%oUc;EEiWOYu;f~Zt4=d`z&Uc=4layL&v2H z2Xzon7?hh1rs2AWd1(hOs4SP*9X~ZYs~^Xy=&2rWH4m(D6!zmM`o*w;wz(}@_v`4v zf)}Vmbd(OOs_!+G@2x?Qrn4R2cUysy79yDy-dr&|c=26kz2de5-`gZLN<@>t>?X2R zis7FEpJq5V#8So35je^4Gl9n#J{V?$ml#ftFnodGoWM62{wnY!!+!)m$MEks+rG{4 z>&F=`G5k&7hYauUW9df>A21{ZNS4}xb!87r+$TjPFw5g{GLf@!heo zNz-$iIk4n4$Ch=lfk+=n0OKKyX^DDnM- zN8l2oL|UaYjS$SW=@dtXK-iObwbWBslZt1~l5w*%p|PpnfW-GoK-nNZ!MpAW3pnbY z;gMbkxY0?4r?KOz?ww8LGO*HAU$7tpoBqh;z+ImA+ zP-^C97IPtnkEx-auRzDeqsL^><$B(J84{Kd62Q!{_D zp-yccVjaiWg-?7A$_YOFMt6>57;Bz{l&O0GI7*@pejf5|>K-gDEa4-c*q;e-pGU4A zNrtmC$>agM1%1WF+$||k4@bAHx>+%G0cy3#PI^xDz)2alC@pmnS~E?$i`z6>r@Abb zOidzl0pEPX<|PQsZPK@soW>(#5t^1q@I7-EH=E&4Z^*`p%)S1G@ANm+lo$Gf?K99f zgq~O3w}A}$BoAi5|3MO&1JT676&lJ`VAt)=KpA(ydLmQXHb}AWIM{x6)a@V4>`Mu{ zWiON2>Z5|PV`TU*!#b&W-qQQ<56$TKel-=DZodzS5ZOIrx&g8xA2FP)sYgbu9udP(fdRFOw`f@Xe)K;LhACO+>O=mv)jgzad! WAJ{s?bjVt0j9T+vDnwx{7W)r?O}O6x literal 2969 zcmaJ?OLG%P5N;p>;Rx1b` zQn_%490EsjOjUkG4ml*hAipH1RORcQktJC!4x0U@-}80PXoLDYjUWvh_FD_;cL%T= z50tfT7`3)Z*R43|(N+=%je5{a$uCw{OMb1m=9m4=l@cv)RyX{jUm}0Ax?WnTT)k>d zsB~+iyjEGK_MKWMXmz8s-w0)MFRZ6=LY*MpCg4S+xxYh=sFw!adPu>;sMCiEmc4$5 zf<}Y3!ykfH+$C6~VUw8BJms0DW$STa&CdKf~Eipnc%B}#+4Vccwjp&KP-3j0d4i7K0I5RU3` z!hVmcVLM2pdm-<{y(o?1E-VI+f_7@StX8g6{0%DFJ#6Y7^fkCw8{}CluCLG;ARqcIEl9)n4{%>l7!uqwSy>TLz1(wgG~|&>y5CNCh-H}Lqv4;cdRbI zEbwoI#{~YT@Ku4Om&9O3V5IPzz-JkedIJAZI4AI5h3^V{a##%B7x|I7kIzzOyQw!scC&P5KRc$#G_z>>2XgoX)>%HOt1#@SF~6b$snyXkoxCK-8D&x`B8hRB5!;dKz+Bsni@YAOOhQVNVP2VOe7?M0$fjqle?=Puf|M-@&RY_yJ zGjWW^U_I;Q$4_Tzx;Av2Y`-{~_D@#!M1$_hWrLPq6jU6?hW|RO&AiS1i5jg=Sg|Lv z-L62Q7k_ZaObc#_STCUQ%= (WAIT_TIMEOUT / 2)){ - #uav_moveto(4.0,0.0) + uav_moveto(0.03,0.0) } else { - #uav_moveto(4.0,0.0) - #uav_moveto(0.0,4.0) + uav_moveto(0.0,0.03) } timeW = timeW+1 } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 41f9b51..2a6a018 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -7,6 +7,11 @@ namespace rosbzz_node{ ---------------*/ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) { + + home[0]=0.0;home[1]=0.0;home[2]=0.0; + target[0]=0.0;target[1]=0.0;target[2]=0.0; + cur_pos[0]=0.0;cur_pos[1]=0.0;cur_pos[2]=0.0; + ROS_INFO("Buzz_node"); /*Obtain parameters from ros parameter server*/ Rosparameters_get(n_c_priv); @@ -32,7 +37,10 @@ namespace rosbzz_node{ setpoint_counter = 0; fcu_timeout = TIMEOUT; - home[0]=0.0;home[1]=0.0;home[2]=0.0; + + //ros::Duration(0.1).sleep(); + + } /*--------------------- @@ -115,7 +123,7 @@ namespace rosbzz_node{ timer_step+=1; maintain_pos(timer_step); - std::cout<< "HOME: " << home[0] << ", " << home[1] < Date: Tue, 9 May 2017 21:38:14 -0400 Subject: [PATCH 17/26] open loop, xbee wait --- launch/rosbuzz-solo.launch | 2 +- src/roscontroller.cpp | 15 ++++++++------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/launch/rosbuzz-solo.launch b/launch/rosbuzz-solo.launch index 97cfb46..3564ce1 100644 --- a/launch/rosbuzz-solo.launch +++ b/launch/rosbuzz-solo.launch @@ -30,7 +30,7 @@ - + diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 2a6a018..8a45656 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -36,11 +36,12 @@ namespace rosbzz_node{ robot_id= strtol(robot_name.c_str() + 5, NULL, 10); setpoint_counter = 0; fcu_timeout = TIMEOUT; - - - //ros::Duration(0.1).sleep(); - + while(cur_pos[2] == 0.0f){ + ROS_INFO("Waiting for GPS. "); + ros::Duration(0.5).sleep(); + ros::spinOnce(); + } } /*--------------------- @@ -58,7 +59,7 @@ namespace rosbzz_node{ void roscontroller::GetRobotId() { - /* + ///* mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id"; mavros_msgs::ParamGet::Response robot_id_srv_response; while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){ @@ -67,8 +68,8 @@ namespace rosbzz_node{ } robot_id=robot_id_srv_response.value.integer; - */ - robot_id = 8; + + //robot_id = 8; } /*------------------------------------------------- From 7ae5e397855d426d01eaf20932c0dad6b384776d Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 10 May 2017 07:34:57 -0400 Subject: [PATCH 18/26] compute user rb --- include/buzzuav_closures.h | 3 ++ script/testflocksim.bzz | 24 +++++++---- src/buzz_utility.cpp | 84 ++++++++++---------------------------- src/buzzuav_closures.cpp | 66 +++++++++++++++++++++++++++--- src/roscontroller.cpp | 3 -- 5 files changed, 101 insertions(+), 79 deletions(-) diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 822c391..e501285 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -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 diff --git a/script/testflocksim.bzz b/script/testflocksim.bzz index 79c58d8..61976ae 100644 --- a/script/testflocksim.bzz +++ b/script/testflocksim.bzz @@ -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) - } + #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. diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 024227c..152f3e9 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -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); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 4f98aed..c64f5c5 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -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]); } /*----------------------------------------/ diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 65f438b..1deee9a 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -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; From cab2cc03a1aea685c8115d843a71bb8ee7efc3fe Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Wed, 10 May 2017 18:39:28 -0400 Subject: [PATCH 19/26] fixed local setpoint --- include/roscontroller.h | 1 + script/testflockfev.bzz | 9 ++++++--- src/roscontroller.cpp | 10 +++++++--- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index 43cb117..5cc6e63 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -69,6 +69,7 @@ private: double DEFAULT_REFERENCE_LATITUDE = 45.457817; double DEFAULT_REFERENCE_LONGITUDE = -73.636075; + double target[3]; double cur_pos[3]; double home[3]; double cur_rel_altitude; diff --git a/script/testflockfev.bzz b/script/testflockfev.bzz index 463ca52..8dc3d2a 100644 --- a/script/testflockfev.bzz +++ b/script/testflockfev.bzz @@ -15,8 +15,8 @@ TARGET_ALTITUDE = 3.0 CURSTATE = "TURNEDOFF" # Lennard-Jones parameters -TARGET = 10.0 #0.000001001 -EPSILON = 18.0 #0.001 +TARGET = 12.0 #0.000001001 +EPSILON = 6.0 #0.001 # Lennard-Jones interaction magnitude function lj_magnitude(dist, target, epsilon) { @@ -48,9 +48,12 @@ function hexagon() { # if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS # timeW =0 # statef=land +# } else if(timeW>=WAIT_TIMEOUT/2) { +# timeW = timeW+1 +# uav_moveto(0.06,0.0) # } else { # timeW = timeW+1 -# uav_moveto(0.0,0.0) +# uav_moveto(0.0,0.06) # } } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 65f438b..a37e792 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -31,8 +31,9 @@ namespace rosbzz_node{ robot_id= strtol(robot_name.c_str() + 5, NULL, 10); setpoint_counter = 0; fcu_timeout = TIMEOUT; - + home[0]=0.0;home[1]=0.0;home[2]=0.0; + target[0]=0.0;target[1]=0.0;target[2]=0.0; } /*--------------------- @@ -755,8 +756,11 @@ namespace rosbzz_node{ ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]); /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/ - moveMsg.pose.position.x = local_pos[1]+y; - moveMsg.pose.position.y = local_pos[0]+x; + + target[0] += y; + target[1] += x; + moveMsg.pose.position.x = target[0]; + moveMsg.pose.position.y = target[1]; moveMsg.pose.position.z = z; moveMsg.pose.orientation.x = 0; From a08e121228e04c978c6c12e59fd28d5044cb3b79 Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 10 May 2017 21:32:10 -0400 Subject: [PATCH 20/26] full v.stig. users --- include/roscontroller.h | 1 + script/testflocksim.bzz | 4 ++-- src/buzz_utility.cpp | 12 +++++------- src/buzzuav_closures.cpp | 2 +- src/roscontroller.cpp | 14 +++++++++----- 5 files changed, 18 insertions(+), 15 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index 43cb117..123cc8a 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -71,6 +71,7 @@ private: double cur_pos[3]; double home[3]; + double target[3]; double cur_rel_altitude; uint64_t payload; std::map< int, buzz_utility::Pos_struct> neighbours_pos_map; diff --git a/script/testflocksim.bzz b/script/testflocksim.bzz index 61976ae..48f47dd 100644 --- a/script/testflocksim.bzz +++ b/script/testflocksim.bzz @@ -153,8 +153,8 @@ function users_save(t) { # 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) + foreach(t, function(u, tab) { + log("id: ",u," Range ", tab.r, "Bearing ", tab.b) }) } } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index c86e7d5..f1679c4 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -50,13 +50,9 @@ namespace buzz_utility{ (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); + }/*else + ROS_INFO("[%i] No new users",Robot_id);*/ } int buzzusers_reset() { @@ -95,7 +91,7 @@ namespace buzz_utility{ buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1)); buzzvm_tget(VM); if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) { - ROS_INFO("Empty data, create a new table"); + //ROS_INFO("Empty data, create a new table"); buzzvm_pop(VM); buzzvm_push(VM, nbr); buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1)); @@ -424,6 +420,8 @@ static int create_stig_tables() { buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); buzzvm_push(VM,t); buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); + buzzvm_gload(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1)); buzzvm_pusht(VM); buzzobj_t data = buzzvm_stack_at(VM, 1); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index c64f5c5..47ee6e0 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -139,7 +139,7 @@ namespace buzzuav_closures{ 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"); + //ROS_INFO("Empty data, create a new table"); buzzvm_pop(vm); buzzvm_push(vm, nbr); buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1)); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 1deee9a..52c543c 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -7,6 +7,10 @@ namespace rosbzz_node{ ---------------*/ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) { + home[0]=0.0;home[1]=0.0;home[2]=0.0; + cur_pos[0]=0.0;cur_pos[1]=0.0;cur_pos[2]=0.0; + target[0]=0.0;target[1]=0.0;target[2]=0.0; + ROS_INFO("Buzz_node"); /*Obtain parameters from ros parameter server*/ Rosparameters_get(n_c_priv); @@ -32,7 +36,6 @@ namespace rosbzz_node{ setpoint_counter = 0; fcu_timeout = TIMEOUT; - home[0]=0.0;home[1]=0.0;home[2]=0.0; } /*--------------------- @@ -748,12 +751,13 @@ namespace rosbzz_node{ moveMsg.header.frame_id = 1; double local_pos[3]; cvt_ned_coordinates(cur_pos,local_pos,home); - ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]); - ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]); + // ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]); + // ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]); /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/ - moveMsg.pose.position.x = local_pos[1]+y; - moveMsg.pose.position.y = local_pos[0]+x; + target[0]+=y; target[1]+=x; + moveMsg.pose.position.x = target[0];//local_pos[1]+y; + moveMsg.pose.position.y = target[1];//local_pos[0]+x; moveMsg.pose.position.z = z; moveMsg.pose.orientation.x = 0; From 2b9c62df0eb8f72eb91bb3facc8fc2c5feba6b7f Mon Sep 17 00:00:00 2001 From: Ivan Svogor Date: Thu, 11 May 2017 01:57:41 +0000 Subject: [PATCH 21/26] update from solo - xbee fix --- launch/rosbuzz-solo.launch | 26 +++++++++++++++++++------- script/testflockfev.bdb | Bin 64884 -> 68074 bytes script/testflockfev.bo | Bin 3676 -> 3676 bytes script/testflockfev.bzz | 9 +++++---- script/testsolo.bdb | Bin 65144 -> 68524 bytes src/roscontroller.cpp | 16 +++++++++++----- 6 files changed, 35 insertions(+), 16 deletions(-) diff --git a/launch/rosbuzz-solo.launch b/launch/rosbuzz-solo.launch index 3564ce1..054d18b 100644 --- a/launch/rosbuzz-solo.launch +++ b/launch/rosbuzz-solo.launch @@ -20,23 +20,35 @@ - + + + + + + + + + + + + - + - - - - - + + + + + diff --git a/script/testflockfev.bdb b/script/testflockfev.bdb index 9eb35cf7a6917326c5e69a8d1e19e348c33c0907..76c007539283d5847bada16ccac08b5a6e331b8e 100644 GIT binary patch literal 68074 zcmb_kXPA}M)xDP30SgvTVQ>bJVPFtJ35Y?uB1DQ&Km~=tP(qOw5iF=MsFW{?QLsRy zNK;UwSinY6QDaPq#$FO*FNx)|px?@IukYE9zs_Z^^XJZ=z21HHUc0RKURb+(p)jEE z<2lei+ZQT-J?1x6m1BojUNC0-pdsf@7&&}&kKv;SO&(u4{(>QueSX=uD`xj}u*PeyKVP@0OK7&^o1<0ce>TnI@bAT*2sFO92gZC~J zMjI6VOec8pYxXG=W*F2V8pwY8778~Q)GivxBlas4mKzk%(+OU@dyPV2lR=5R?1>T^ zZjek*Zau7n?Lf0dX`~ap;j6&;xk2q_pSkWV-J?(#VKxUBY1;&py4Y+=?K60l(~ z3<~J!1R*a2rB-Vp*ML&%4e|v040_qiQHmGO&Eadve_L~d49~S)7RdjCy$XdM_FPf^ zr@Lg~&vb&3J+Nk>u)(0%`Gb%jpvqbqB=VL{ZopqhvO4diZ6_qFkJ++2=0111OEhPB zuI=$ibcH?lSo;iya3LU1H7NG1Amk5#9Bh!tc{)MJ2NC7P232C8LCAlB*6R#Pw4NR0 z7+$9FvKl!dv(fC*$z{ujt~xm3a&4!=fJ`fP(|jXPS#7|bO1mc#luj_wHvzJrL2*e4 zLKc8C*dPzE&miOzfc&jN9i)N$9P{fMgF+0_33~aiD;klPvmC?OEpk~}2TWrRr9R2E zo#q%`*P@ea`=(i6-lIr=@yoyxav0Ki8nRSp_d%1<26` z$vGT^eA3mSNS^%B+Yu09O(&Ot^|1K!X2oic>9DP^=kd#r z+~>XnTh1eP{9M}uQ8e5s(2~;$W@Qo}sX0nDkS_s}9j2!S@X`}&^^;Bzas(i$kVJ1Po&_Y|RFL>VFTX(mXWNBEv226@E;mTgn~INJPh-^vFJ9O6 zG&Y1FoeA=eG#jQi*R~|U(ox9&czdp>d9?x# zUn__xbps%0YP}o`9`Ut;XtwloltNh339@uOAgMh?#P1KL(K`|GHvkfjrb91t$=d+r zrHn=Wq?2pg)5&PeqM~fqIT_9Onz_$y4LV+8&*OcAi9XlSQ8e*qqiL=&FGURB7)bO{ zNHiCZVmn+fmuPOlxyicACA!4Ik!!mJS)sNRh43qooUaWCnd{EOF8oMtI=Qx`2!E;B zuvT+zXTp{XUa__E0c<%*XaZ=-pp}Y6wCgY zotw=&5-{lH20&7fiQ4z|u1t`rbaKn&9BhQVnJ5nzqf8o`8zRWIxRzKi-QqY6E~XPq z?J(f*jR1ed9)$cIh`h$Uriq%!`HmL+F=;S5+W;w+mDf6^Q6UGt{0@Dot;L@sI5!WQKuxeJ9TGp+_GoO^ z9R0+|wfz*fo$a}7&$+hU(e3!_-P(o`4B^u*(W0!B;aXxjvjd6lXcuNn$qiZ)m#ks~ zJ{z_b_S|A+)MqZyjBKvYM`FV9d9)2C`eer)Q43uINdEF$q?ZoBszANOstjKIYxJGF zEQg|Pp`l|q>L;C`mvaJ4tLNClQ5JOP9Y8jh^mKxdFFU4n&_LeiB!{TPj&)2E{a$|%ia@Ln9tR{fuV}ap1*Z|tbja;s z#^-_4tqqb*HMhjhbaI6ymTUWvlPgEr^K)%GIJqL~G)o*yMUbP=VsEyaA;{?jlXU}# zzs8_MKf@aAh%fSTlOsOA_X=M8Ne~~X>2L+hWlR46OE&~qdXi(Qs7w7Bl%fI_<^M)U zDN&c&70KFJVf0iW(aphMDqv2UT#|eV+s;NXuB*AW0|Erw7$8_LEU#4F{E0xY&JMv; z5lfxt9cM)Z`yQMXtEY0zKE5A-!|4QLxtl8kcGg@}0KUWl*f`K+v(TT_3x5 zI=MbS9H4S3_E)}J5WOw@0FX`X3S4Z08NJ)7MMgFVd8jKm(f5b@g1=&E_k$}q(cak< zIeFO9gRrI(jO8LQmYFVAo}C5q_A@t%wS5G(BX`oa%n`rLFoI#I24rJ{5-H6OT`h^e zi=PS}O)xJ-kZ%BzuSq&+V!5BbmfE0}_?m<*FPQPgAU-!t432biyY_GxP*UCFFuO*sTKIz0O-%A89e!Z&)wz}N%JSR{OSAo1oTT~GHbb?pu>NKsWd98HhEzx*c z6Suw2pt!{bFMfd|FH(|D5b_rEsx<~Bise&!@_q~Qb~Z1$^X8IwkRvZ((#f^$i=~j_ zR`e3-1>}%#PehQnx*Uqu_iZkRWfrs`)Aqt7T#OUX`DYC6Fiejgz37QJ37clFRjbHjhTdJu`52T%`Zn|B~xI>9{DbM??b136t^ z5AOhyZ%9Pia|GtY7Q1kX=Hkm;iGkkf*e&J<*mkii6nmAe zt~}W_f-IejS~}X?;Zhfbyd0F8Xpo3~Izh;PIhOX;K=wcYFEOYR4dk7GoT-I82t(mI zgA`?aG9Yg@NY+I#JtJMki(1`HuEf~mb1QzR6PXNJuI*ZvL(x^1VCA^O0**10PB7rl zA>a>aA-AJc*#L@Fbw5Y4sOpMMR)2(qs-AmuFgmY-;kQ|I*kXf_dm}W|Jp8pu5VEz? zJp64!5V9t6NL9~KmrMM=x%~5|1G%VZh@DdrspcsjwLJq$?ko?Z&qhM0Gv)OG^S3WJ2h=>#vn z+HJs$Wo~&+0Mj_>MV;nFK=S8i_%oeccXr2k?qW97yt%edyA7B>$_hsII=2DyE7Kt4 z|GHY@e-0Uhe8dr-J53O>8axv3=^p};YVveTanQ@ta9i=7ewco#o~mD}$71Acu^5Ur z(KW7#kVLta_xC`>FUEe*+OEQ`k^9`?ZkaE(TRh{s1$UYt9p|I8%k3&$b%T(zTpfzm z)sOUb*c*ho)Gkh)5WM&w0XfqkkG9VsWF1%9WJ?fo8gOng??l6V1@;56A{>XDY&9=A zkb@WB4oHejs)-=vORkBK>bZ^JV?C$ubp&jxA>dnJi&%TEbZkKk(h26Ft<#>Oo#!B= zwwrlJ9haNhUw{jxX3IXEYx{|w7H_*8VuGX-40tWnl2|{sb2&Up<7I8+@RtU~CX$P`>vd(bC=NcD;d=QW` z?ZOqBi`RFxBzlv5xvM47xVqB`HnJrc%gL@h!Q*s-kT2`YvolY&$gbyS-zKyAlw0v#9n(t8om|_R7^+>&hSABj zZHwv2?j#b5!$AC=<__4EPLTM$9Hm5~?hsJw9`ladQ_#!%Q0CMe#lH2s1l-|x6uDE& z(SkwCrA0eOi%yyx?gm;^7?fyQYy-~O2Fc(Bb6ANMJJcXWWAa)+ij~=VFpaM{L7a4Q zQ~M7Xa8G7?&b3{QE_J*;uh{2K03G>Hmzk{G=e`JAv;e#3kDtLrFLJVhMH_^C+R>4b z4MJY$xFh=U#98Rcm;M;S|L^=kFU!!esOoDQbP%!{kW}?#OAzuNFz62RPL!Uh7$RE@ zQuI!AO`yak21@MjuEZK^a=6%)SVIlu*^a#RG>{*GX}1}asN??;OuN${QNZa0$@?al zMy(~PhdUh8@Mk)?4S7<4Y2P3uViofkXi;rf5N%H%180drGCIK=c6E}Y+@OPy^)ZDh zZppOV9QMGqO3e-arYhHVGV(v@C(+5Z9f>hXeIWX(>{~}E(E#d;8eU;Hc|&Ypy#TFn}m-MJi@5khj9z!Jp{_Y0(Pg z9coab0o2ctSG1QL;m8Yg#l^cj@;22#ZU_jYD?wf|Of*E^10=(ASuCl!ysg9eLdU=NJ?{+;m;X~NX+b*Ha`{L7q!WaE0gzNzB1*MGhN{dv(M-Eludc4e zOrvfQt?x4tz^!(1k;wP}kQ5mlSHX;La<#1nni6|)CeO8G8=eAW?P)V%uWg;7%k|E!(N!(7;M@i2%&km2_>6$ZtZ4ME6P07(fZ`bM#XlVGAe+yET0UV73g5ymn%wO@c~+_m_b zYrDZI5tLOrxwZ$p{EK$HR{_bDQ?!tsg#4dxHv@V)L6$DnYn~hRl==+$d`NrojgBo` z8*|w*1I>%6EtYBT!emmVvtVI z%LRa3WKj4sogn1*z*%BYWLgk%i(a720g=T5WgwVSY8OA+K66WKrYkW9E!Xw~S7M?U zc6YlHYh_mn=5P$^;Yx!Nk@qivtTsq4NI@@;cO{0NkWLWt2S6@0DA5$#>gqu>Yv;Op z5J9%Ua>5aZhD)rG^Wtt;?Qw?Md z5Pyn6iGJ2z<%lop+Wo=O$IMGGB%L5Fo`p~>GpJ%;V15H1xJL6cmlo$ZIySNA=i2sh zlSo$T}3ss${(+mJhpEfT^sbG5k1eUVW zN&H;4d>dft<6tSjSS@w~e%s|=6xmhCKffGh(FP-XEBg5&OP0vXfyn=J+WdbP$l<0y z4zI!BV+|LZ|MlonT+2jqWdf$ul@?i1OL-BH)dpE&pFv8U07~(VfN1dTj@vFZ?@(6h z1igG3kZTP}^u_9RC?Lvr&cmRWI|9Vt93cMe0P#PAEw5jpN!IUo8x+vf z2`1}jj-?{vKjT;``WEyUuvDy`jsr_6s(JC;@w@=GtUuAV@I4^;S{F5tPOdv!V9>^F zinT3wG6{Jr>bY}ZJJ@Wn?4*DM;l5^?GOoLbB4l(RUa&^ zFn7xBGZ>xA^~CQ19-VLAi6quRu9igqQubRAU#!~ximt=v0UoE5%a)-5md*o9N7(Z) zl8buonR=F90ZR2UH&`@5mc9!}iU?8uPjxI6_0qL~6l=>R$mb%9B^SbAEWgtef4d`o zu@k=v24z2COK#Eh2YLC=LPSiv5=`r1?ucmdo+B^cNC$Jc&M~c_c^ibh5H0pfgW~2D zgnSi{)dpFj@p38ZVX;B6)dju05%sX%AVoiL+5yN728raS6ZCQz%7mjI=;`E^$(NW9 zY#zi2x-$Tk$N!L$?gSw>0p=AEdE zJrBsW2FbNHc=6r9X^Ko1b1tX1!ImPEdC0Y`L_P4kIEsv1+pkpemf21 zMF`0ZgW@I{^l}XHztSK@E7z| z8--{2J` zvHv7)2W(klqSrf{T|IO#w}Lmk%#j!UB%L7S$za;=4N5ewCW2|J3{vzBTYD$EMC)5e z5OBD8$wrV%-eoWtV7BlqonRgofPfiw#o5v=dzpMHRo$ z1+CZ@7)@Q!TG~y6IlK)wq;6@Xf!q^Zpl;y}gI>PjbPFMKnKlnW>tQ$J3-+Kpp97M@ zxkMARhcMr$a+_-)*P=}>HYom>DR}W$&?d#p?KGs8y^No^sjY!B=j(2kXRhry*s=i? zJ9)RmmWwfaY3_5Y+!jkJ2ea~pqoau7{oNJ|8l@BTaswcLYfvJ{_t5cY86=pQPSDGB zu4oR?KpqU&mKl_2w|Eth>kJafPbYZsny!|}%Un)h;A)ATCfD|Fu9mna<=VEzEamph z7nr%WyP^)cJ$KNAaF{D?wv=F2o^lI$CykdgP&6|PipdIk`92`8Hb|r-ogic{)FF3# zQE5*B&MNaxREOO#v4$IDvBPT>47iYs+-Vo6hi#5&o$X4&9G2=A@{9C2?1Tw^pIsQc zNIJobcXzZvdrK#`FP#v`;Ud_Mu;;M>4WNc3ZY{hR_4EfN0&HJjOmi+yj^K0v8s^t6}{$R2i;Qu_?%u!ak`Xak;(fU~9Z zGq;xBfo&_hS+TY!B3XURw%Fd$(k1IyO=Q0TRI&b zthxx`GK-;T;=ct*3IT5I!Sr12h+ixL4}t*&Id{w4qL~ca3X2Y#X0Gkau%#d`Rz&Qn z7Xl-l5a3mUSsA1+n#Wzyh)Vlv%;XuCL*_qt@lODGjX@%;=>#GBI&C2;n!^!5acTb_ D9EHh~ literal 64884 zcmb_kXSkNtvHdJjQ2`Y!pu&N308fE~2tq&%(xpW>BubHXI3yH7`W3-kK#C$IQH+97 zP?{74G>Qd8rKqSeCPX8ms0p!`#Bx=TyY6<@edgiUerNXn^PNAl-YIKl&w9TPHXfhL z^~wEs&XCU*xzb;cxu&9Y_@u$3OUnmO7&(0Oz{%rF$B!#79XDqDkcm^Kl#VYSH~iWO zr4uT~PZ&08O!>%R6_XkbnKGsNPja~$f^xQe))%yiLne-%FtPMUb3hFrT|R2!(2CMY z73EF-&-P8&wh)`sGTNSWVlKz$wG~@?6r+GvM^MgnqrC#OLP5FQjW(!KE_aQf;LmXE zk=}B0E;mb1&T^ymJ|&mCLr{)!qn&qZF1K1x0341z($1B0xgCPy_pLj+X1JiS3b8!j z1lzu13(ALMk83|5ekv%($!Dx5D^JMfMu^Rs8Ep%}oM~c{FQ4`(et`ntB`5$6$Bs4~ z%vqldZ3CFIMbIvhPrGkD6muA9Y`xw>ty_u*~nrJwsIoe}-ev z=NVOUxvhdy*NyfOTBxa@pitq&X83JXr@hg(9jenyY&q*4 zeA;WV6lh%qg|o((=KTqxgTA85nX1DVs2MBWbbV0SF zJqNVOf>OLd4htV!?FAJzUm0SZ#O9pv-e}UR0CY%96U$jinX1lI9xNM?K_IY z_%oc?=T=twrJdN?YcW>o7yq5OSLv7TZr_dphX)A?_aA$t6+pX2&@kY`v7=d7n!f@t6;Mbv;>a_w73*>lX3NpSSu*)@niX>-tZD#<|2Ev`1P~&7~6Y(T=td za|!N+gOxL!*dBiZ22?Ro1HG;+oZ?9k(2K`P!^YdYmVkW2&PAoI80y9U74J#OHo9E!+DAanKJl8*kngW7K z+|QglfHpgsZ-YP=Dslf^&|5JFDGbNX!!1Cgj`Op+54eO&IUGN$w*n3Bg+t$BnOO(S zq#8tXgcEDqU8zxAAen48DK*NSF7~<2z`7gcd5qUy*};l+{(E^orr!qf&ClU&j>^_W zWw{w-omV|nWx0RGR%f%SY@TEy)^-%J$XuHp1Vw}Z9X!eFOjn^ zV{PZawnS{R_QglAZ7;T2eLfpwh|ir zJA%n}OER?ofCQTat(JV+eXETTzfDm5_vu{4bMBn6J#voXD(@Y8TvL;B^#ITcB};zQ zutsr}Z|v=no{F*!PFBIc0*zX`z|F24CO?^9)s0pH=C2VH|Az3T>Z96j-xlLm0$@1o zqgduo1H1S=H1BjfAKppI)v*YMI)(AsecP+pRnMLCKP1&D$AD7v#V0gqIQB>zDz5tT z)(2PGPVtHCvir6bXjDD^Nqf6uJ_-^}Eb|9oA7ifhX^-m$Fu%Td;74njRFB-Pxayx> z!vH}|&0i+l^Y%Lsbc5uL8?_y+FW9w9&`{oR>}cB*SE*?2Xk|(Pap#QX>Z3{laSM*M zZKb&6p9a5D)#on>?K#;4v~=q1R>dW1OuKJ~Fk4y*hj=r_=6nap2l9kNR>#_2fIT)} zY>5$T`#Egei!G;XtZiqEtwv&7T|VvEdr?)^UyA|+lTIldMrGSdgq#wwSt?Z3$(rH$ zuq~12W>qL(sLC>*u|A)VyNODiw)V-+7^&d{J*sx2y&;}$)5!p^`rmx849kV5aICCUeiV3YBNFO zB#iBvtCTum*TmXBsnh}A<;2>yQR=`yH&!Sf`q4&VLjFPWiPVN;ug)D{^#(!l|B`2r zVzuA59g5Z5)a;Qy4^{&(9Bx6eeCXrg;Z_F^FH=19&zwIi_2IAeor*dBnR6_vV^tq7 zI4au+G^MWO9TCfluVLF>7{!e-*0!&MQQI7h>VYMMTN=N$w)fOoxE|6ei&qst{fznn z{7k1)ig0yNYs1ZO?0Gv*v7K`=mN|PA+q2$@rHbuYZTo?B)TCI1!-@6zDF`<1vH&6b>h8e_w9)4CjU-YjBa{Le4{F|`?d^p=~E~wEBYL*K{AN9`S!iJQZ}ILiD2@02;pyZ{yC} zsY!dJ1$tzS_{2Av_L?pStGV-GKxU5XVl9J?oracfu=+|}rj0pdZ{D4vEb z?^;G(#+|BkDSq%9PmIMeKT8w<2_db0vo(Jt{V&z-mDfyVb9 z)cE#DZ&y6$q>Amh0S=zu3}!ZvT#)bfC^{<5=$}Gs6*K+ys)F!tl}Ncm+9MsOn2Gd< zV@JDN=~;i?UP#K!@4?LW5+V1@SY{4X%mhq0v9@JOE&AWUyoP$+D<1gK?osvfZ_9gB zy$ZxfJC{zvRmZm(yt!g?{t9g2OgL1*v9?W7;C!*=o5on%Lk^DhRy^_d&K96iX_D>s zTAYUh@EwJpC$B3G*OE-zeLEh7<~s`i-riWLDnD8!#bNG{v0OUr;8G!OLrw>OCw>96 zBB_&~OOJt>N2^79_0Lbr%;`W&r?XlsEtBWw`6`E&84VtChNAhxvFH2;2Qzo!CZz}T z$nHsz;(4Jvu74|@`xV6F;CX-X$scJgKyU)raYs4>Xz6%<5NLd#;MZ{@Fwb{Mgn8~r zSF5f8ZNrJ3z+=!gd?)6=3zp*Xd;^emuQ&qR4&r^*D0Zuk;~ix0xP|DE2I2`fEIZmv zFlU0GA=%;B(f*})*wc;H70J3mQ1aYp4+3p=GPG(Kq11r>nob59-%e1o+p92A^{;Wlw7qCNc5+s&^ix!a zS}ALF`XCXFBnrRY*$)oiCup3I_6%1-LZ~VDI|n;jGo>l`{kR>i3hG5&%v&jz)qhj9 z<~P@|wk=hyv+|@bYTaC_bBui2Gtf5aUVSoZ{gC(s;Bf4|y@m{PXJDkU87@`ZY?0fO zZ<21a{ZPTn#V1^|;n=hE6wuP`#`$n)i1_5MU^_tEFKBQx9DAhe)&9x6#qwc-;*x)E zybd(}VhMkS6YI(GxJf&R4b@w$?Tc#vtR@)teBP$^Pkys!NBbYuBQ@N2%y$#Rmi(jy>lsm5%f89@S96PU2HmrSLP9JYQ@%S7WRH zS<>pir|N}aAC5i4Rna5qv{)-uuexsEs-s>%7Zgr|*qk@T&BgEInz%g~gh6?^pdkI> z*mHgX_(1hUJ!1E52GG)N`;$QT-I9IY$ade}09v~J{B1bITC&4pyK=Q- z8jMlXfIN?V?qAe2sPAU=QZ)^j343*}RIK)YTl56bW=n)6?nrB^9`V1YovC`nf3rNO z6cG7fufk;2aUf|pcC>v-JFYz(q9!6kVwwMBQa-$o3f?E#;f`R>(woTA6N2V{L((Rx zCaQrcXVpX<9QCR|BDzXWu*8L9&+u1hv;PtlB-@F$RdK1I8?Ad%1#~9DqrRqgvFGhF z#iaswq%{?nI9p=-x1-`xo_G>#TLrgX2eDyJVr^UEj-#&kE3aW-bvN+@Bnrn~ogRug z{#)rRFy~?M3FFP_+oNb&?(|vTefk=?wlNl>K6-&f6ng# z1ixYT-}|LX?=bgvX5N}q?`#2=_;wG}2q(6J|9}B^Q>vR-+x3_^&EeB9cf2doc;f$dzCT7PbusrtyKe<30d;Y8cNOLW zjk=h8u>1C~+WGxen2O22TO#zo58UABnu(6C`Mc_x2JU(-S6x%jjdrzSW-T|`C*ab3 zg5same*~8v6g0GBICf^f11?d^_<8<-;u8K0Cw7WXa&YN8BqE(&cm~y(v#(wfY&i%0-`ZCPmpX_C)iQe02Db*) zqF*nZ1ZEbC2lz7_d-a=wndO4QY}wIzD`xt4hVv9N0ql&lvtlOmXODF2ad!1F3(TbE z^iTc|fW|4ntuMAmE>p~G@6K=~#Z1n5dxrlEG%8ufWk+k4l;?K>jo;kWcIT}SBuJ+z z<^zqI!k@S4Nmal$)ivBwV!8UE;yJq~*7mKWDqvMo74Rx}ezkDOKMgJhGlvSAzXz8n zp69uFJ|wB$DN5Qk3xIY{vb;Tx>nfdknxT5o|E}gB(5S`ONA|h;5Zah;!?JE@Z=)-D zuk&rN-ILX-E9<+tT8?0*3yR;j-9VcoX#O5A2E<}PJJucPe}J|^P&kwARd_-%-yi7} za427V<7e#5p9*$S{bgn50od}FOx*VEIk^@SzMDkh*OEB|!?zHAEjbiuGsQP%$sXx* zfFRG=*>B>`qbBCW~}YGj#_t8weI4sbyZbsG)Fjg4!;I8 zY8F3pTA?IE#V7wAbWc*vax3m2>H^LHd*0?CS-T}t|0e$t(74I-X0iLWL-h!&5XyOj!p8)K~PdFZc>l25oFPOR;E^jG2k+x+;4J$uW*x`~2@WQ1c!Ym7uJ z78L)6za5jAYLpqa`!*M>dro}wE2EDUhy8Q@eDI;uQNG3UVLEK77nr43+ke8Ay!ZcN zVS{331Bu37ui*B{!(cfvMbiM7I@xMp35m0YdZ-$LpQPI4r|Zn3E&}BupjMZ z#cKZ)>XlS|6eH37jX7TkTfV;#h@3s=wH?);tg2rmuGl>}QB~jno34TA^tIv>+ziL= zTLX0Z!-C@XtxR#rzqc<^eDKe-JjDkLm2hIGK{woS?AxwlYtMOgl&x4$$ag#1n?R$w z@qdfmMyVTr&hG$3Iz95d+WDEc*b06HE>RcrGuC#i+WFBg;l$dWscP-twe|yTfOz2F zjxIy3hY1>h!?E*lWl~LXXHw>TfwDcBEYfX?58T{h`7j$(hQe+Aj@`EaM9c@=q z6|evVO{W6-ffo4^=>_r`+ck4k*Dy=5wjZgk@xR4-NOet9iNap5G2r=)g5qc9Uw}48 z(EPhxbJaCy?r`jp9s$})LGj-;yA{v<_uxXsb3a;RY#LSt)Zft~A1j{Mch_qwiquU| ziri?Yf|=C6er66uk=7>TQVFV01ysi!=~YP;(1nUiphh^cb8j(hxg{gSaP0kiwS$@U zz)Ze};f)y^MNf=HDz9GdD5fb^7rW6agVomwivQQh6^hmVdD{m(d`5f&5yG)o|79qz z0zt|82JbsOo{Ec~vDF`-SjSy8*0z^oPMJFcHNhM*#=k|p3$%1Pt|RC&Od|C2@M{bU zY9;^N=nEdccocK&Jp2=Q$SRQ4v3&U6!NX_4Lw@U*b^m-%)!LuWb*MGJxnoD#^LY=( z@iM87-?x6K^()D0eaKO-9gce4g4>D;J!`GEVCHa6`PIP$+%GpuK5;pOW3Tn=K%+Xy zbE91X=1^0ElHu5WJ09V!6cn_J6YWKyZ4?y$w|=*wF{pxhf7v5F>|pgy2dn2hSp5ZT z`QE9-U7g2NkNE#LlGfni?cx)F!?9QACyIxDR==cp=>MMXLhvx1PPrI7q)KI^u{Y@w z*jATZa~rhh{0E@%%{T}iPOK-pV9-Krvf37b)l?-YRHo-HfbAf$!AcTNtj{wr;JS$o zcNu&3&Pb|3Mj#Pu#W&u4cHf$Sv5yEEIc7&|t{97;!->uLQrNZ?8!Rc|*gYu+)$#?U zP(EWlSqSC~78~|XtnHN;aMZ$nMN=C*B>#%UBYRF}CS`S3&}EqTfqrb@Nk4=7*}XGu{^vgDGzT1bGnNM>;q=<{j#4+v%sY;;)%cdA1G#Wd$RL< zlj2f6@zIWU9VX;5k(rH~CoGZ&Z&o zl1$h$@RVY8D>vHpNW?5bfmjL0?%NpDdaa=Nx1i=2M(K3iB?ymi9{Agy*a|L#Ep?o~ zBN{6%W%Z;0p2TJE-!d@sSAyc#p^qw_`}dajQ1ZFr8>?@Rv=h1}-Nw64^&oEt zyKhwiu~H)SPx#k>#{HSv$nM)o=p*jW^ewiJMyVdmx&yxgTkhUSb~v%mJ%o#m8ndz3 z+A}anO#?<@N86#c@t?VU>y3^ZB`E&e{J+s=vjok*+y6uLufIn+C+*+U;Ls|G6e$eH zUen8vtigi9DH_Ywl}ahFzwDkg0||PHC;UxitS5(6*ZALl>{L8&BOck~ny#3M!4Qre z?Q(GG_k!ZTStf!@>jcgJ9Z+kfV*DFaTd-@m_{NTo<-;l%^buQbQ1+ZJ0lP*B8mDNi zCq>wKipADH&ugLN(*(`WrAt-4{QbLBWhtxDFIHJqa^jwk492toP$y*m5J~JdE|ZLhX=bteyW~Dc1Qp+(+$@piDS+-?jp6vY`0U zKESM=D`+5KICkGQsg5|yjdms+S|up{dw)OBHVGQa8;(8FDym1ww^*)@Q$51D5o`N5 z)g#yu zs~xV`MZ`aUdv!RLY~J-68qf7xCV;FwuyY&Gf<#v?eFxd zsP%gB3BcjleOsfnhW{JtQNrnb};9{q}5*xv@Q}wzI@v2Ras@& zzkkj^hS8qk@H4hY4#Kvn(e_eQr(-+JL0{{4+`S z`A5_Se!e@>nn>0vLGjiq;{lugj_(-*ZGYWex1}5!@XH`e|yLBF}&skD0*4iHFXF!`LXh>l=cC<2dEBASS KN1U6qTmKib#r{|T diff --git a/script/testflockfev.bo b/script/testflockfev.bo index 3cb83de1bcd58d79ddabd6bd34d7d4115e4f8bfe..44e24c101645b38d3aeb82e7f0452cec5e55f235 100644 GIT binary patch delta 48 xcmca3b4O-FIg9cF2MGlU1_lNNNgyGiAO$4l7#JKJB^0C~%mWUaXR#dO1OV{92_^sl delta 48 vcmca3b4O-FIg7G`gM@+v0|SGCB#@9$kOGo&K%S$7f;5CV!Ey5}mP4EX-ZThz diff --git a/script/testflockfev.bzz b/script/testflockfev.bzz index 463ca52..92a4fc0 100644 --- a/script/testflockfev.bzz +++ b/script/testflockfev.bzz @@ -1,6 +1,7 @@ # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." -include "vec2.bzz" +#include "vec2.bzz" +include "/home/ubuntu/buzz/src/include/vec2.bzz" #################################################################################################### # Updater related # This should be here for the updater to work, changing position of code will crash the updater @@ -11,12 +12,12 @@ function updated_neigh(){ neighbors.broadcast(updated, update_no) } -TARGET_ALTITUDE = 3.0 +TARGET_ALTITUDE = 5.0 CURSTATE = "TURNEDOFF" # Lennard-Jones parameters -TARGET = 10.0 #0.000001001 -EPSILON = 18.0 #0.001 +TARGET = 12.0 #0.000001001 +EPSILON = 6.0 #0.001 # Lennard-Jones interaction magnitude function lj_magnitude(dist, target, epsilon) { diff --git a/script/testsolo.bdb b/script/testsolo.bdb index d25ea48913a362e840bf458b3b0f8a48eb19684a..e3f057f93e9187ddb79beb03aa2646dcb0363b36 100644 GIT binary patch literal 68524 zcmb`PcbM1JvBnogutQYr;^Hog3ri8DC>BHz6$JzfdR58_QY{n%7)xX^m#V0MiD*Jm zKmiLjhzcs8kFY+9H-d?Z8c}1pw%D#l@0)YyeeOK|b-uiF{`u`c^Zw?{d}hv>vkNsl zmz2~k`M<@_ijvktuNXDB^|(RfE+0Ft^`LPRC$=6ly1MnS%d1C@yL52ts|Q!NZ8d1( z#Qk^;my|Ryn|{d zb0ydIO-EzHg0?5u`+t1)D`>kLNIt-9I@_lZzEQ3)SdfC;E0C)+uTIgu8@O9Z$sm(C zSeIK5xnU-=hc0&*>g*nqA$mgWdZH22yP`e$VHjLut~Ahh)QFEr_9m=YA`#IhN#-dal3q>jDW8H7Vv$+ykj#HHv2dIv`aowl$%RJ zRLVj0y(Saj-P^$DHIoa#=Uj&m_MKL4&bA}p$Nj>UT-)VBzURlzSl?scQ*9do-REKq zMKesU)IJ4#o`D>nC|KWu+!XBae)B3|#yT5BKW1_)Tfw`705{(h4%B1!r7$PG54gLU z3#gH_a^XG$aC62&`U=^uaD~BJb?*0`7(1UWaCOEDwvgOs5sG;h4cs5n66B^jk^>5( zk0UwY3jY}70>{0Z-Fyb+v~sEV%2feR`CQvcFsQQcD|-F?K&YUTfLUivJsQXhcu zrJ;q%3UYICVgwFUpMhk)Gz8Sy;{fDI+hjmt+~Uwfdrm7CpHk$GO^p3K*S5x?cW}Y) z{m`LDd~&~cKR0=uV&5yUGTu#IN9b~IxXJ4*U2fOd!gv{SBh0IS$?FbRXZ*t%Ar&_O z-@rj`777EEn^w^q!c#EdFKpJ0hkOj2Mc7QB! z+^Yl<_u6LJ@`TL(HkR27!QC+Pj9Dqv*o<%Tn+ zBDVxqy5WpF%e6fqwtN=5;VfYC{y7%VYG5?eat7tJ1O%Rk97jjyOpv=78uy!5?1F;a z`M7ZN5hUQZ>fhmB!Qbwpsil<**@1wJr%u4h-8FU$D?u8k*^YQ)65{?(r13$M3%FN2 z89O(9@6yN;$W3EY3@tl4H%)dF1yPhrcW9l1&?*Y+Twcc|GQl4%Lay%us! zOr}hidk1orCUcf9*Uc41!2SI$0Kx}_0Fu8(VLW7;WCsya@sukJk}Sy0L1DnbwAcl5 zA$t}EoSTr*v;&mv&q9Vv`h1^J!VKWotj(26-O80Ikc$;Z50Vnk)$OVpFZCq2~1we@1 z?a;X2HXCqaUWA(E6vvCF;N5zm3sj;D@aj&;a;x!N*9G|ZZ@IRsTo)*{$mZI5Rg zY#@`C5WZc2-ZYazBvrX#kYiy4pjQjIz`~dfmzLQk5k_^hKjGxy0|E;pH;o${whUCR zZF68-X72L<%(a~f+bXjydH{V1+pcC?)aS-H@cDi8S-|#2$gz1a6oT9tZp;jLGiVj=qg za+MaSp7tqtw-6{yGMNC#o^xqrGZVaKn09HjX)(|EeM;i z*$I%VHt$O9Q;<6ux#KfKD_yP~N^!Bt1e|XJ(JKP4@E779%RhrC`p>AJhV5|M87~C6 zygv-0I+!hv*tCS4O>_tZ5cL-1c+n5=t_z6zP26TLhf6a}j(uK;U3&-U{w6EP{Q+{z zOeWxpGYGstZgK&gY-jY@<4lg{TQ0L3-Q2)qI@h)}t_fxKz1UG&xwhZJmUn-gScPnR zA!cpNCZPN50raND$z1`tTTBkkBrO5GS0TsCdO%_P42{6cdR^et#Wsl_)5=Aljl-uX z0=L7q%D%7Yt^PZQ9&bpwK9}PT!m*RKLUMaMk^|b*67=Lr78PtVEy25gzlI00@&e?Z zGOuu}RK1%mAkfsMkv9Y(P@h7sx$TG~3vx?c8Us2=9mM@%^A5^s3Es8D4mq!M(&c7B z?m3eQD1ikai*1sx2f4*P9370ualmg1=K#=_wzHzg@>CF2VYb|ckh6o`a8jn{>{gev zfSc(E#2pT%#ZO_ga}oD;CdX>bMW8?GjiUn*$hCbAw!`gv8I@ey$H84kvkkalUXL(d zYI3XwA)y0Js~*UjN5JZk8noO*}1u#;xx9AArxFV&UEmH16p2{}qAcc4+ZD z#~78|R{Jm5cD2Ot?w4yjP^h5wLIs^0J11;ML5{W%;K#HCMt|ihDBueBI^^y)uXt|~ zyqgc?mzx}v(-P!r(S2VunE>y0cNGLM)5@iBv#X$@mzDOef{NM>0?B>NS6sIX`Mg1> zg68O_9KQLFnuSb$3AyIxKJSTw+?yo?9fyo7UR}WS2 zaGYFQ#Gfh|MO6(z&oLK@+CC545j$wx!ogjxhu~(&RhUe`0Qr@x zm4Nx=aGW~)Wmy2+^&O1?H^RAK=Q)cV2RVVpC7_YVB06?jxinr01D?Hw&8wo<@F5P+qNBr2$k|}~-hgxE zBgk=}4B)+|Lx3lzkhA^a(lpyF=LbRVUN=yJhO`8^&Cqz!WO(5e?1J>qx| z*zDU5Q2dyd;N6}8l+O$SgIz}ey2iX>Zxp;+j@vztC$O4UE}-LJKollq;V~i;5uky(^!9N55{d8%BUxzO<3qT-#<20on?HPJ`TF z%UQqy{2AmpCk8wf^u-SOR1k0%x*ojWYMTv+-TesIa+3>qDj4H<=RYzQGW$ozdzE=9 z$i0E%f(^0g=-3&x!da_mt$Yrm#+v&9)AKowsB%3jhk~eSCKE9JEdx<2Ob*Iv2|3$} zgl#k#oNKCFBOt(+2i_QR+t!;7AHtn$d$Pl)D0%~N5FKS(59lCYfW}{&TtEj|1&v82 z7eM1-U}k!pciSPy$B2Mt{<@so1+sbOiS=?Ja+iF$u!pG7UASxXEJQlf_LvYxWQ^#xq!BGY2+nY zaAg+chQ#gc0@OJN>T;76yqoN(;GbUzau3ChKNB2ehwGa?7*upOnW8tuQ{4!|){{%r zH$Z?hM?mRa=n!bCN9710Fx_N$hzJPW1i9Nx4u@x2g51N7sDRkL1)?^ZR{?#diyL7$ zdkC971c367F7abpxsY|jVa5@rt=S5$tN{u&CWDNoCCHUI$O7a(h6V>){;ops?f_5` zICE@++$M_|(10|Afhb^RxuH`y7qLN093QqRPxG{(dc&ilbfJ1M2Jt$Z-q{xcKx(C*dUC zN{{MC*G(Kz03@y4#&DJ+s!DF&asee@>EI5K`vP(c%{%5>pz$!k&5=Feh~MsNCBVDW zU9Ip@M%e7rm{)iWF8XkJ6Y7lf3Q~}k;L854eAx1G%cqMgpHp;Kc1QWNFqwdh&t_!=ZE)2X(9F-n*|5Ym8^C*GS3UvFd<_6S!o1^AlS{?NFyM|D_gveRsMQWT_`TPF zWd02&Z}qv~yA8H1rV9Ihp{N$Sd4+W@$lVz`V4UyJ3t(ju(0jsS7Erbipw+H3Ii^I2 z-6H}~zY&Of+Y!}FCu)f!s;Ms5(M@6je+56!0UGdoy<3p?z|pD30SZUc5{P;bfUY;0 z0Nx*Ufa1rra@*Fm0-$wpo{hKf<9$Mi%3#QGdhTqpg50cF1fFt}7+);}?<#P_ae5|E zxjE~DB$ru$n%HNq?RCg^f3qpd%4o34;Uu814srSJt>?QHT$*k&0l%)e6>_spju&hp z-`_xP;V0=d9hXCv}nW8MX{%hs;AOHEeT>}`m8s)uQ*>gq8RtwlWRLlpHK(PYs+KjwG%*PKMQWa zyfztHNQ%HwLz?T%!6p8rc|fZA(E^ zH%mQ_nq1rQZmL1oRq@#s37TLsh=wXR(&hVXUG8Pb)tF4FE_V!WAJgOH?tmPJBsiLu zP$hAE}ps}Z;@eB)vP`3L!8Uy$g2pFIhQG%arIZmN|4*=Xk>rSCF)y8BQKA+wrdbs zjz6gWv~q3BK=P?(1NRHru8f^^wgJ84&4oJlDP-gM*lFTspts7rf^u4dcjbtCcaz~} zb945eu;p-4G~b`VmV6zgZ{rb%Kr>zLZ6MIoWZ2JzAa{kvz&N=jkh{#}0uXos8V{Hp z_MMin*`v@w&M}#yL|u#yQf9VHV=j$NK@_jmlb@apFB%?2z)bT4QG z&N^2?jtf4iZ^wJUWo>jU@2^2_vIQJ8q$R}tI5#I9smtB%%7Hv-K{GY{OtNPKzo`D zdQ(~g0?W}hI87pbs@yoo`}w+DEvV#h8t~FK+wmT-*+Br5^Xied*<3(pIzWrU{iOpm z;2)8!c7XCA7Kl0nMDe+Ur6x8(^MY|fspSSFw@Rt0;2MU ztCfI1GVYA%OgHbU^vy1Er5Et4u3cT}p@XF*q~aLFpYIa^weln6YRxOwis0RV*h>Bj zhO=_>uINL39c+hvou9eoGuV}FrG0;{?FFv%`kJi}(IzOp7A6yLk2*QFRz?BtrRH70 z+x0-;^Nh&_98>Edx8CFeX1&)yBU8asFPDm)1R7t2EuRWl^10vp5yIHpLIC&E5@?)= z&V8-P1RMli9E~k??>2+Rz)9>6*dbpYKvG&l>^3+W+0+ENodu#c3Pjz8YYAU1iqg0y zc8dELaPx))Fx3RT>hK9T`0oThtIaELu6kEj$oD28-?t0-{tmW$rK-|Hu-4(je=jIh zP;Xqv_y|Cf1-VBcH`F#6a26g2q66pC6L4bjO#zOkm3xTHhwXl5!)urjs9iwgK_-VE z)5>+F76u)6(Do=dP@&WdeeZ184lo;@qq#m$L7NJkp3ZjI20RTMfnY@Yu*L?*ET1*lIQW5%j^Q!b~jf7 z?moj&wq+()X`i{S%t7g~O|$#t+FlYnj8&kLIRP}cZ3r|@b&Vn5Umm{eN-sceG-_q3 zZ8G3II}Ww-sLAmF5@I(5R6b*J0hRnWG<9Yc=%DA7VhhT zPvC6V9ow1li=7Gi9s?ThGdaXNEkSOh8^)UJa&I_%=v{6aN5Y`oTqxS#1^@w`va}VV z@;Mq32dYMTRBmuY1svX6fdI#QzUT>?{Sz8e;C!|zmZ(vnqRLVLN7KqB>R)Iofn(P| zK-T0Jt_X2o4>{fe0;afCI826`cLB%L>khJjJJ@^&8I;o!Vpr=R<8dX(?GDIz=P!Et zY=$i-G1Pflxq!9@pzJf)Q6bx(9NewURtUicM`I^l?mW;q&}0H8g4MBbH%6i+n0JTk zn_U5B0*AAGP#}xqVpk7j)tX#DC7*(8Asxy2P|sV?^+nEjo}1PmN! zg4r1+7f|mHLgP-8+ezQ-c;xOrlfi8#Eg==1T?qti_8m8KvOseo`x;d60v0gnO$HTw zItUo_{sFlu7H|Bh#{Db@8LLr9;{-Rk2iU6)2~HaCr0Jzf1wH<kh8atGd>;gUl|E<_afMQItVDA%{YBFSo#70Z2&4b)o}*NrDBXD z3SJc=Y71;R5wgGKe(%npkxv$F?0ba-UFP8CHUznikgGJWPSU;G=4cGK_$_uc29(}u z7*;sK@`%q(<1#ns5IrI8$Gbs?f0di-N?k`(z%$b(hd^f?fs1frbI@Vu76P>va>L9k z9wLHVIW+iAP^xeCK=6K#$pqZ4_e4e4nq0u$|4PU`V{!qPm(x)KOa*>SE0@{jFu25Q z0;rhp#t?SFTvv{AV@N;Gan|KQD0sIQa`%#L zmJL~uyUOt%a9k{Kkg>RPA#3X(Yim0cb~eXB7V!7J{|(5dm{$SUvOaDM3Ai*q=9t9@ zk(Lm<>#)Q7OeWxo@>G<-VnG7Zdf U5)YW0`Z{IuiNU&N(E82|tP literal 65144 zcmb_kXPB1NwH*|}4pBj|z~BsnQ(yq4C>ErMjbcZy3Ns@J44^V#03#Y1%%y`0m;fde z9ZrgFeFeC?%4oP^XHpCd%e4?z0Y3n_d)f6 zIXU%n{&V`3FRsARYiqXSv9#cAIaMhTqn?~I9@92LxI49>4K`3$|O+Pdz=XOD8?m~L^ z@SL2-1Oe`aV~cJj61!0lPI4jbIWi|_yCC4taBMy`QG8?VV)-^+@vVt?5o`OV;#=d4 zw&ynZFH4TeXnP2_daT%VlTTabMk>~`+iXa$0I5)X>EZIJ?2w$CazQxWg;WcqVS;d& z3+W{E&x3+MhS~De7sHf3Wu2=}z~FlEqLC|}?hf|tQkCxM@}eB|sSpGtI2>D6w*#qC z5Pb8k2s3wrAhA9+pPDD*%&i!YTarZA4d9v;7kfH-pi43yF9cF2@!WS0{1ZH$Do8%|4FKh42$F9v zd|Gt`#(Fq5_Em$&j|hVAR=8O47@!WHj!``JAx&{8v`=7rn#93dF181Hp$9G%TTanf z+tXDKoGKW$+SUU#(x|LC=z%mU>+dLN8kO}p_(qjQRcg!Ao8a4a$u#~9$A&aj@hxlJ zkB0%@2xL`R6Jc8-o;Q`xSiZdjTfPzCO%`i=BnDy{l{FejtRf?@)!+cd`eMn24e1`R z{tiL#-60<;*8Ar5UJ&#J@rhH(7Tr3SQz`lOvlA3^$%a@4{msF?Mv8siU6rm<>^s4Q zv>Ql;f^dNg=|&KxLJ)kn;0_>F3X+eS8Ea;4W0XgIO# zTMyfTVgn+EWAox&n4c~PxCFw9^GeyH%qRA@|Yn=x$h3eNWenL0WSD(Y)DfTSADF%RB_d}F+T;Q zG)iig(mwzVC${ciDxUK$jcxU+Lz<6S zpGLKO3a(PC`}$`c$ndm85C9sE&8Ox{8}Tl+A>9R}t>TMM`Snw3hZ&D$^%|vi7*DM2 zvB^53MyVa2cKs;RL%IrEE#DUSzR&#@ z_dCC9#5f8kmO;mXLA)D$cWKXLY9ZMrk+m9E43{>7eu6_m9KQ2*m9@sMejAA{xYD1a_*2ArE*we8L+5!V=5EL#Pn->L+ z{&`1nrmxG3*HDy7LGV@j0TksfLGrP0zN!zB6i#e?2BJQUhj&PA)wi{GW=Sn_fZC?lLZxI{7gk#IxL15T4K|m56 zNW*|c*83P%2c$Htp9@D;N>oUhBf1|k?y0`X`q*l1SL|eFVr`4S&OGs)w^pp}OxTu) zZPr`k3)uD)+pIn}#|6#j(PtYw?*bAh1G8X5x=<-N-~l`a*lJZHGyJxgH>k~v50d5h6wrphttxO;YdVm&3IgwVTXc(5 zj@j?AIUbBVg!+e{HZLY4$IOTi>2a8MlFay&MH5ht8lOzEMK>Nu72;E_eAZI7$R1j@QzXQ@rLGW!E<>2u;LGsO@12GrR z5G39`v0U7t_Hy2yv9_hSxbx(>2qv6Z+izgY-5QrmCfj~US39xs&62~wuxUww3 z`PdJr-e_y9W`D(1-{e`2xjIR5fzZOS`SkBsziD{!0+5~&U$EFYe46X1bSqUY-s85+ zd;+8*iGy6VA+1o=^38*K$ng{66F|eU`P2p>Q4e%=AAZiT~7TXgf0 zNUftsu1L02|%2D4WQ5VyMyON7?D)7{8yj0`h5YUBJrF% zoDJzp^j>>G!UYqK4QVyZCkXH(i7Xo0JO-}v~L+feb%x1-Gm zMYc+wsFrMeTMoYQ2E)J&CzfwF!+`gwuM-zyURzb?4Pfr|;(68xUVts{N=9PKg1}ABhSUgGCf{PPOJgV2ZrCyo{>nbqwkt@+ZHnIx$J!p~ z;Q9X~Q#5P9ut`z_Mq;bMv5H~7_pN_~o4hdlA=eu(8Es%-?ffL%6)oT@x`R}D|NNW|3ebK$8*oQyEvH5g3*hgjVQ%YUH zzD?o_=bO!^Ral{UlYoff#IkQJ4ES_kW)5=jxDK{_I=!*wqP^;fLdk^<={waCzPDbz zk|~QLz`ivSm2bCOq?85w-j=7;ihWu4vs=MFI1-L;r(c3cQ>SOWwHttay~K;G5xk_B z%gJxc<2>-KwRpkZ#fG#6)Zn+yzFA&?W=x}$CZfHmN_-uD9=d6{BfKE50BQy@`u`PB7g2#M<2x0Bof z9^WI;`SSD#lCw&XeCoPN@tFS#(^k9RD;}4KZ#JYiFxff9vMR7{=mF}MtUd4XI|kDWSiDwdGB zv9{+bmSkmE87_|Y618t0d=9?-QjmP}U<1r236hU*CxIH%llZhBNL1~<3H-X^8&VdI zjk#xGMo~BUdSEZu2e5EBD`MH#2~9)gT_hN`czy&D5jgE^Nae{`y%AijmZ*F=eoryT zcgr0G22uO?xVT0s7IHDR?zbt<9ObI}J|J=S;LmVky*NxM7S<=$_En`=d=*&$=C&5k z`ToJi+@Dkr_%`th)dRd0Y(6~?q%IPj&!>xUYcCfhA9L3#=AuQziLLuv4!$i{s)U@4 zeeTUlm1O1WpA=WQAK0q75DY7njQOT)2}Z^MLE?sM^JzAa(kP{BB$lcKWDCdU(>R2* zP7s>Ake11Entz5Y{8I_(c2%P-3 zO5Y8ndj$zsTsStQClqIVdD;uk>=0jkv!c6F2i)y#(VYPH@z-_uGn`lkb;WH%bH6bXVaGed#IzVP+?P9UY((l@J4V~%Zk`dIac zZ`bS(N0v**eahiD)oDK6xU!xL;+5-i zpZgcZw*tw6t+q=P-pP@i!U0a`R1TP_Bv`gC(4or3yI7X%;s)~ovX-VwhGqU;o(P^xfjc{(2* z&To)>du&U^8Bidc*beWbIFp;vwh3;9USfk_!ijzExykgvNDSHz@^s&}Pz&>FLGpER zp<=EN>2n}06`xpJ8{bX>bE$HDH}8Jc13sTFP(8pm6}IS}g&yG6ne{E*-RK|c0i-(| zn-@)0M{r`rc0_m85j|X99Ey%876jiGvJ2+i#C@C5cGX+H34A4H`ErTQ$K&RzBYYEh z6WDjM_{3Www(cLnfN?O#v9@c_ew~fay$xLD@ASAo$3Ax-Y}qLV@_bvz)F?f`{elr_SvT_SM7vGftkzyWW?Vjdq&dkd{fyE#)c7`^3UC`yf08q?^|=%U&6A9@l+Re(iKy*BvB}Dd(V!$1 zfNwSqQMK*ws%=|1GF=dSZ{P0$(p*8}W^1eM*FakOvmD!yK2!=IR}gJTeU$>>24zEP zt(c24h7-%aX|Ux~@NK6jfXBV$=|^YuqIWU{&nfO^DoC6Mw)*S_QW`C> z1Ldt2pM2AQmSes z$&s1QfMnHN=c_L5>GGnn>QY~C?NPnO4B6OstTje#XjHfKBp+{!?zo@j(AdO zA0N_2P>yeFvTi8vfk8-4IKC}#wPH|#Alfo>HtK$L5}%eNV^9MmHjU2Pf^@Hz=yF|o zx(apA69j(7PL4^aJ9SZ3&b$v>Y7g$Bwo0Fa`jklo_%j?E(o$7vYD61S!(^4N1QHd! zPxXD8Oc#~lwxJH^)QRQH0L3?Uc&u$MIMYii%v&MWcAU}*7@7{29E^&L7X&22fiyzZ z_7WG;%Rs6Ygj^TWX;=lOCqcR&NK_qgFC1Gp{Q#t;fnp!D3d0P2XY zrGwRdlI{CV)V}YF=S{Wm`zCuw#YJDW3K7y;sS@0CRO<>1kIjPMLwZr|`@FYer`gwF zA2lMs!;iH+*}=EIifkM}0yTRFC>6%rM$sc>x3%>>`p3j)74wIOXp za%u&M^Tvj>L-CFCIF>WtD86xPi?!W^R8m=>p~H!_%>!4@6C1dn$#zXLEwK*_J5!!s zPd;rmcs|+QJ_`)nAie-J9Gg%1$Z>B$V05wd`Cr&l0c5T1$FL=rnzMC;Ja~psoHXq+VXe=%+qLzTYyBJc!gxf=F^{nR4oX(E~G^m3278S zJur8v_~KIlUtw(Uc63<2QX^ZCs6uVduI6-VkcL3=$K|-s9V?(M+ z*0WtvqUqubqqO;SBl=^7AhB9Dq*B!%xcI}dA#GAx&i6}>mz9txm2RJ)wUYc=qL!jT)d%r!1pWBZb-*;@u|cW-7?ipzHej>R^5aF z6pk(9ryer-Lxz z*z&Yp@r~2ThIF8VGdmodxfk04HGWpUZAzw%J_2*Ovw(VzioB{=;=7p_fF&En7m(QD zQ+-EmcRFf&pQEe=JB;(B!HOX)Tb3N>skP>p0Lgjn+F0 z*9^5d+zTgm@-K$%F=E5*$d;Ldz_(_Cgg?WH^`Z_2U5vKvm1;o?XL{~j*bWjK-g~h= zPr>9#qf;(X?DQ$@W{Oj+g{_)z18K5k#;2WDU2P8hROw*ud@y&oJdYb&>~jYuW9}VbNgD0c8$rzYSvjtyytQanX2 zq&E~x=u>R9M!+Cno}P6^4ML@P7t+?2i_b7ws9Ku1a&f2PjPG{cgGy5+^IexMx<6pD zq|rY+lW}Gw>Ruw5hI`?}a^_!{4rvrk85q>^XI|KHTni-L&pvIm0mFZ&_~g4$URMnA z?Lvzcg8&+iEl+idLA+UPNQZ(!+^MtP5xZbZeS?M#CzgGkz&_3j1e3}3N5$N>Vrz?j zyW(3{7t)pBTbUsEcJGbJnA;ph881Gya7DKo)JUUtjso$QCCO6*(56n1e0|viOBvrZ z@Eut!k85DtTfFdXiJetPWc8vrnL3!2OdVVW9+yjII9Y66`bQuQ6D0f@jt!}W>WEwy z()&P~EC@dB^Juc(IvTz8p!np=(>b`Q(x|A0s!M&})KvoMIf;(_VPnBUblPx1;yoPO z?S)B<4~f91py03IJS&; zQ>^wy_qNh%?6g=0eTBMn^YN*q$*4P(wNEAe9Y|9oxA@bM<35T(>@8ch#;dxcVI4lb zqpHQ|VyiV8lq->^gFfNJ+77{XnMN0VtvKWRmHgd`GgSSy`s_u0sI2+#IBZA{A+c1} zzK+<1J7T+3$;ZA%s5^BL_xaemS1HcGp-i0F4O?m|&ZyYu9tgfs(YKT5+A4CRVlE@F zA?*NCq4;u+%cp&cZ@!JIM)A$pP3L3#r|RR)8(XcFN;!~Wwj7_SlmmZn8S6!T#Tnll zmz}E8-CUKv7FQ~j181%+Gn;`lOnl+ww;|=joLV~972R>*@q>ck+hY$$Yt{*pZ@0b~ zNY4q9ZrIaoOUTrx@h> z{pLTwpef>uZ!@}7ZTP-z?J31Ytmxs`@-z`4JuC>mw{Yiy)lUhMkJU$li_a!OdSB_6 ptX!P07?da8*mC?mkf>XI`~Ea=k-F8VK`v8V^!590R4a{g_ Date: Wed, 10 May 2017 22:04:04 -0400 Subject: [PATCH 22/26] stig in flight bzz --- script/testflockfev.bzz | 32 +++++- script/testflockfev.bzz.save | 216 +++++++++++++++++++++++++++++++++++ 2 files changed, 247 insertions(+), 1 deletion(-) create mode 100644 script/testflockfev.bzz.save diff --git a/script/testflockfev.bzz b/script/testflockfev.bzz index 8dc3d2a..06b3e32 100644 --- a/script/testflockfev.bzz +++ b/script/testflockfev.bzz @@ -16,7 +16,7 @@ CURSTATE = "TURNEDOFF" # Lennard-Jones parameters TARGET = 12.0 #0.000001001 -EPSILON = 6.0 #0.001 +EPSILON = 3.0 #0.001 # Lennard-Jones interaction magnitude function lj_magnitude(dist, target, epsilon) { @@ -145,10 +145,32 @@ function land() { } } +function users_save(t) { + if(size(t)>0) { + foreach(t, function(id, tab) { + #log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo) + add_user_rb(id,tab.la,tab.lo) + }) + } +} + +function table_print(t) { + if(size(t)>0) { + foreach(t, function(u, tab) { + log("id: ",u," Range ", tab.r, "Bearing ", tab.b) + }) + } +} + # Executed once at init time. function init() { s = swarm.create(1) s.join() + + vt = stigmergy.create(5) + t = {} + vt.put("p",t) + statef=idle CURSTATE = "IDLE" } @@ -199,6 +221,14 @@ neighbors.listen("cmd", statef() log("Current state: ", CURSTATE) log("Swarm size: ",ROBOTS) + + # Check local users and push to v.stig + if(size(users.dataG)>0) + vt.put("p", users.dataG) + + # Save locally the users and print RG + users_save(vt.get("p")) + table_print(users.dataL) } # Executed once when the robot (or the simulator) is reset. diff --git a/script/testflockfev.bzz.save b/script/testflockfev.bzz.save new file mode 100644 index 0000000..4de66e5 --- /dev/null +++ b/script/testflockfev.bzz.save @@ -0,0 +1,216 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "vec2.bzz" +#################################################################################################### +# 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) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 12.0 #0.000001001 +EPSILON = 6.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + 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) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else if(timeW>=WAIT_TIMEOUT/2) { +# timeW = timeW+1 +# uav_moveto(0.06,0.0) +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.06) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) + s.join() + statef=idle + CURSTATE = "IDLE" + vt = stigmergy.cerate(5) + t = {} + vt.put("p",t) +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) + if(users.dataG) + vt.put("p",users.dataG) + table_print(users.dataL) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} From e384bcf83b40caf8bb9e5db3ac2dcd064e821e5f Mon Sep 17 00:00:00 2001 From: isvogor Date: Wed, 10 May 2017 22:25:18 -0400 Subject: [PATCH 23/26] merge fix --- include/roscontroller.h | 1 - 1 file changed, 1 deletion(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index b9ac6ff..5cc6e63 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -71,7 +71,6 @@ private: double target[3]; double cur_pos[3]; - double target[3]; double home[3]; double cur_rel_altitude; uint64_t payload; From 61d31e672da52e84cf7969f10eac07e12aa0efb7 Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 11 May 2017 21:17:50 -0400 Subject: [PATCH 24/26] change gps mgt --- include/buzz_utility.h | 6 -- include/roscontroller.h | 34 +++++----- src/roscontroller.cpp | 147 ++++++++++++---------------------------- 3 files changed, 63 insertions(+), 124 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index aa5cb2a..381b417 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -21,12 +21,6 @@ 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); diff --git a/include/roscontroller.h b/include/roscontroller.h index 5cc6e63..6dc1859 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -52,7 +52,7 @@ public: void RosControllerRun(); private: - struct num_robot_count + struct num_robot_count { uint8_t history[10]; uint8_t index=0; @@ -61,18 +61,16 @@ private: }; typedef struct num_robot_count Num_robot_count ; - // WGS84 constants - double equatorial_radius = 6378137.0; - double flattening = 1.0/298.257223563; - double excentrity2 = 2*flattening - flattening*flattening; - // default reference position - double DEFAULT_REFERENCE_LATITUDE = 45.457817; - double DEFAULT_REFERENCE_LONGITUDE = -73.636075; - - double target[3]; - double cur_pos[3]; - double home[3]; + struct gps + { + double longitude=0.0; + double latitude=0.0; + float altitude=0.0; + }; typedef struct gps GPS ; + + GPS target, home, cur_pos; double cur_rel_altitude; + uint64_t payload; std::map< int, buzz_utility::Pos_struct> neighbours_pos_map; std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map; @@ -159,13 +157,17 @@ private: /*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/ void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ); - /*Set the current position of the robot callback*/ + /*Set the current position of the robot callback*/ void set_cur_pos(double latitude, double longitude, double altitude); /*convert from spherical to cartesian coordinate system callback */ - void cvt_rangebearing_coordinates(double neighbours_pos_payload [], double out[], double pos[]); - void cvt_ned_coordinates(double neighbours_pos_payload [], double out[], double pos[]); + void gps_rb(GPS nei_pos, double out[]); + void gps_ned_cur(float &ned_x, float &ned_y, GPS t); + void gps_ned_home(float &ned_x, float &ned_y); + void gps_convert_ned(float &ned_x, float &ned_y, + double gps_t_lon, double gps_t_lat, + double gps_r_lon, double gps_r_lat); /*battery status callback*/ void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); @@ -178,7 +180,7 @@ private: /*current position callback*/ void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); - void users_pos(const rosbuzz::neigh_pos msg); + void users_pos(const rosbuzz::neigh_pos msg); /*current relative altitude callback*/ void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index b5832c2..300fa2f 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -7,10 +7,6 @@ namespace rosbzz_node{ ---------------*/ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) { - home[0]=0.0;home[1]=0.0;home[2]=0.0; - cur_pos[0]=0.0;cur_pos[1]=0.0;cur_pos[2]=0.0; - target[0]=0.0;target[1]=0.0;target[2]=0.0; - ROS_INFO("Buzz_node"); /*Obtain parameters from ros parameter server*/ Rosparameters_get(n_c_priv); @@ -348,7 +344,8 @@ namespace rosbzz_node{ uint64_t* payload_out_ptr= buzz_utility::obt_out_msg(); uint64_t position[3]; /*Appened current position to message*/ - memcpy(position, cur_pos, 3*sizeof(uint64_t)); + double tmp[3];tmp[0]=cur_pos.latitude;tmp[1]=cur_pos.longitude;tmp[2]=cur_pos.altitude; + memcpy(position, tmp, 3*sizeof(uint64_t)); mavros_msgs::Mavlink payload_out; payload_out.payload64.push_back(position[0]); payload_out.payload64.push_back(position[1]); @@ -440,8 +437,8 @@ namespace rosbzz_node{ armstate = 1; Arm(); ros::Duration(0.5).sleep(); - // Registering HOME POINT. - home[0] = cur_pos[0];home[1] = cur_pos[1];home[2] = cur_pos[2]; + // Registering HOME POINT. + home = cur_pos; } if(current_mode != "GUIDED") SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm) @@ -521,9 +518,9 @@ namespace rosbzz_node{ void roscontroller::set_cur_pos(double latitude, double longitude, double altitude){ - cur_pos [0] =latitude; - cur_pos [1] =longitude; - cur_pos [2] =altitude; + cur_pos.latitude =latitude; + cur_pos.longitude =longitude; + cur_pos.altitude =altitude; } /*----------------------------------------------------------- @@ -564,48 +561,10 @@ 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));*/ - - /*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; - 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])); + void roscontroller::gps_rb(GPS nei_pos, double out[]) + { + float ned_x=0.0, ned_y=0.0; + gps_ned_cur(ned_x, ned_y, nei_pos); out[0] = sqrt(ned_x*ned_x+ned_y*ned_y); //out[0] = std::floor(out[0] * 1000000) / 1000000; out[1] = atan2(ned_y,ned_x); @@ -613,48 +572,30 @@ namespace rosbzz_node{ 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; - 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; - out[2] = 0.0; + void roscontroller::gps_ned_cur(float &ned_x, float &ned_y, GPS t) + { + gps_convert_ned(ned_x, ned_y, + t.longitude, t.latitude, + cur_pos.longitude, cur_pos.latitude); } + void roscontroller::gps_ned_home(float &ned_x, float &ned_y) + { + gps_convert_ned(ned_x, ned_y, + cur_pos.longitude, cur_pos.latitude, + home.longitude, home.latitude); + } + + void roscontroller::gps_convert_ned(float &ned_x, float &ned_y, + double gps_t_lon, double gps_t_lat, + double gps_r_lon, double gps_r_lat) + { + double d_lon = gps_t_lon - gps_r_lon; + double d_lat = gps_t_lat - gps_r_lat; + ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; + ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(gps_t_lat)); + }; + /*------------------------------------------------------ / Update battery status into BVM from subscriber /------------------------------------------------------*/ @@ -713,9 +654,7 @@ namespace rosbzz_node{ us[0] = data.pos_neigh[it].latitude; us[1] = data.pos_neigh[it].longitude; 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]); + 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); } @@ -748,15 +687,15 @@ namespace rosbzz_node{ moveMsg.header.stamp = ros::Time::now(); moveMsg.header.seq = setpoint_counter++; moveMsg.header.frame_id = 1; - double local_pos[3]; - cvt_ned_coordinates(cur_pos,local_pos,home); + float ned_x, ned_y; + gps_ned_home(ned_x, ned_y); // ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]); // ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]); /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/ - target[0]+=y; target[1]+=x; - moveMsg.pose.position.x = target[0];//local_pos[1]+y; - moveMsg.pose.position.y = target[1];//local_pos[0]+x; + //target[0]+=y; target[1]+=x; + moveMsg.pose.position.x = ned_y+y; + moveMsg.pose.position.y = ned_x+x; moveMsg.pose.position.z = z; moveMsg.pose.orientation.x = 0; @@ -850,9 +789,13 @@ namespace rosbzz_node{ double neighbours_pos_payload[3]; memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t)); buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]); - // cout<<"Got" << neighbours_pos_payload[0] <<", " << neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl; + GPS nei_pos; + nei_pos.latitude=neighbours_pos_payload[0]; + nei_pos.longitude=neighbours_pos_payload[1]; + nei_pos.altitude=neighbours_pos_payload[2]; double cvt_neighbours_pos_payload[3]; - cvt_rangebearing_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload, cur_pos); + // cout<<"Got" << neighbours_pos_payload[0] <<", " << neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl; + gps_rb(nei_pos, cvt_neighbours_pos_payload); /*Extract robot id of the neighbour*/ uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3)); cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl; From 664e7e4dc00fb8a5222ffc9ffa031ecf16f544ed Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 11 May 2017 22:12:59 -0400 Subject: [PATCH 25/26] changes to the updater --- include/buzz_update.h | 2 + script/stand_by.bzz | 2 + script/test1.bzz | 1 - script/update_sim/include/string.bzz | 92 ++++++++ script/update_sim/include/vec2.bzz | 107 +++++++++ ...estflockfev4.bzz => old_testflockfev1.bzz} | 1 + ...estflockfev3.bzz => old_testflockfev2.bzz} | 0 script/update_sim/testflockfev1.bzz | 44 +++- script/update_sim/testflockfev2.bzz | 42 +++- script/update_sim/testflockfev5.bzz | 208 ------------------ script/update_sim/testflockfev6.bzz | 208 ------------------ script/update_sim/testflockfev7.bzz | 208 ------------------ src/buzz_update.cpp | 88 ++++---- src/buzz_utility.cpp | 52 +++-- src/roscontroller.cpp | 7 +- 15 files changed, 362 insertions(+), 700 deletions(-) create mode 100644 script/update_sim/include/string.bzz create mode 100644 script/update_sim/include/vec2.bzz rename script/update_sim/{testflockfev4.bzz => old_testflockfev1.bzz} (99%) rename script/update_sim/{testflockfev3.bzz => old_testflockfev2.bzz} (100%) delete mode 100644 script/update_sim/testflockfev5.bzz delete mode 100644 script/update_sim/testflockfev6.bzz delete mode 100644 script/update_sim/testflockfev7.bzz diff --git a/include/buzz_update.h b/include/buzz_update.h index c8bbaf0..9c51601 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -129,6 +129,8 @@ int get_update_status(); void set_read_update_status(); +int compile_bzz(); + void updates_set_robots(int robots); #endif diff --git a/script/stand_by.bzz b/script/stand_by.bzz index bf2ac78..d51e99e 100644 --- a/script/stand_by.bzz +++ b/script/stand_by.bzz @@ -9,6 +9,8 @@ if(r. data < l. data or (r. data == l. data )) return l else return r }) +s = swarm.create(1) +s.join() } function stand_by(){ diff --git a/script/test1.bzz b/script/test1.bzz index 3e9d1fb..7debdf4 100644 --- a/script/test1.bzz +++ b/script/test1.bzz @@ -1,4 +1,3 @@ - # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." include "/home/ubuntu/buzz/src/include/vec2.bzz" diff --git a/script/update_sim/include/string.bzz b/script/update_sim/include/string.bzz new file mode 100644 index 0000000..4140a1b --- /dev/null +++ b/script/update_sim/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/update_sim/include/vec2.bzz b/script/update_sim/include/vec2.bzz new file mode 100644 index 0000000..e2fb9b0 --- /dev/null +++ b/script/update_sim/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/update_sim/testflockfev4.bzz b/script/update_sim/old_testflockfev1.bzz similarity index 99% rename from script/update_sim/testflockfev4.bzz rename to script/update_sim/old_testflockfev1.bzz index 1845eb9..02abd4c 100644 --- a/script/update_sim/testflockfev4.bzz +++ b/script/update_sim/old_testflockfev1.bzz @@ -1,4 +1,5 @@ # We need this for 2D vectors + # Make sure you pass the correct include path to "bzzc -I ..." include "/home/ubuntu/buzz/src/include/vec2.bzz" #################################################################################################### diff --git a/script/update_sim/testflockfev3.bzz b/script/update_sim/old_testflockfev2.bzz similarity index 100% rename from script/update_sim/testflockfev3.bzz rename to script/update_sim/old_testflockfev2.bzz diff --git a/script/update_sim/testflockfev1.bzz b/script/update_sim/testflockfev1.bzz index 1845eb9..e628847 100644 --- a/script/update_sim/testflockfev1.bzz +++ b/script/update_sim/testflockfev1.bzz @@ -1,6 +1,6 @@ # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." -include "/home/ubuntu/buzz/src/include/vec2.bzz" +include "vec2.bzz" #################################################################################################### # Updater related # This should be here for the updater to work, changing position of code will crash the updater @@ -15,8 +15,8 @@ TARGET_ALTITUDE = 3.0 CURSTATE = "TURNEDOFF" # Lennard-Jones parameters -TARGET = 10.0 #0.000001001 -EPSILON = 18.0 #0.001 +TARGET = 12.0 #0.000001001 +EPSILON = 3.0 #0.001 # Lennard-Jones interaction magnitude function lj_magnitude(dist, target, epsilon) { @@ -26,7 +26,7 @@ function lj_magnitude(dist, target, epsilon) { # Neighbor data to LJ interaction vector function lj_vector(rid, data) { return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) -} +}cd sr # Accumulator of neighbor LJ interactions function lj_sum(rid, data, accum) { @@ -48,9 +48,12 @@ function hexagon() { # if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS # timeW =0 # statef=land +# } else if(timeW>=WAIT_TIMEOUT/2) { +# timeW = timeW+1 +# uav_moveto(0.06,0.0) # } else { # timeW = timeW+1 -# uav_moveto(0.0,0.0) +# uav_moveto(0.0,0.06) # } } @@ -142,11 +145,32 @@ function land() { } } +function users_save(t) { + if(size(t)>0) { + foreach(t, function(id, tab) { + #log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo) + add_user_rb(id,tab.la,tab.lo) + }) + } +} + +function table_print(t) { + if(size(t)>0) { + foreach(t, function(u, tab) { + log("id: ",u," Range ", tab.r, "Bearing ", tab.b) + }) + } +} + # Executed once at init time. function init() { s = swarm.create(1) -# s.select(1) s.join() + + vt = stigmergy.create(5) + t = {} + vt.put("p",t) + statef=idle CURSTATE = "IDLE" } @@ -197,6 +221,14 @@ neighbors.listen("cmd", statef() log("Current state: ", CURSTATE) log("Swarm size: ",ROBOTS) + + # Check local users and push to v.stig + if(size(users.dataG)>0) + vt.put("p", users.dataG) + + # Save locally the users and print RG + users_save(vt.get("p")) + table_print(users.dataL) } # Executed once when the robot (or the simulator) is reset. diff --git a/script/update_sim/testflockfev2.bzz b/script/update_sim/testflockfev2.bzz index 1845eb9..06b3e32 100644 --- a/script/update_sim/testflockfev2.bzz +++ b/script/update_sim/testflockfev2.bzz @@ -1,6 +1,6 @@ # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." -include "/home/ubuntu/buzz/src/include/vec2.bzz" +include "vec2.bzz" #################################################################################################### # Updater related # This should be here for the updater to work, changing position of code will crash the updater @@ -15,8 +15,8 @@ TARGET_ALTITUDE = 3.0 CURSTATE = "TURNEDOFF" # Lennard-Jones parameters -TARGET = 10.0 #0.000001001 -EPSILON = 18.0 #0.001 +TARGET = 12.0 #0.000001001 +EPSILON = 3.0 #0.001 # Lennard-Jones interaction magnitude function lj_magnitude(dist, target, epsilon) { @@ -48,9 +48,12 @@ function hexagon() { # if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS # timeW =0 # statef=land +# } else if(timeW>=WAIT_TIMEOUT/2) { +# timeW = timeW+1 +# uav_moveto(0.06,0.0) # } else { # timeW = timeW+1 -# uav_moveto(0.0,0.0) +# uav_moveto(0.0,0.06) # } } @@ -142,11 +145,32 @@ function land() { } } +function users_save(t) { + if(size(t)>0) { + foreach(t, function(id, tab) { + #log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo) + add_user_rb(id,tab.la,tab.lo) + }) + } +} + +function table_print(t) { + if(size(t)>0) { + foreach(t, function(u, tab) { + log("id: ",u," Range ", tab.r, "Bearing ", tab.b) + }) + } +} + # Executed once at init time. function init() { s = swarm.create(1) -# s.select(1) s.join() + + vt = stigmergy.create(5) + t = {} + vt.put("p",t) + statef=idle CURSTATE = "IDLE" } @@ -197,6 +221,14 @@ neighbors.listen("cmd", statef() log("Current state: ", CURSTATE) log("Swarm size: ",ROBOTS) + + # Check local users and push to v.stig + if(size(users.dataG)>0) + vt.put("p", users.dataG) + + # Save locally the users and print RG + users_save(vt.get("p")) + table_print(users.dataL) } # Executed once when the robot (or the simulator) is reset. diff --git a/script/update_sim/testflockfev5.bzz b/script/update_sim/testflockfev5.bzz deleted file mode 100644 index 1845eb9..0000000 --- a/script/update_sim/testflockfev5.bzz +++ /dev/null @@ -1,208 +0,0 @@ -# We need this for 2D vectors -# Make sure you pass the correct include path to "bzzc -I ..." -include "/home/ubuntu/buzz/src/include/vec2.bzz" -#################################################################################################### -# 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) -} - -TARGET_ALTITUDE = 3.0 -CURSTATE = "TURNEDOFF" - -# Lennard-Jones parameters -TARGET = 10.0 #0.000001001 -EPSILON = 18.0 #0.001 - -# Lennard-Jones interaction magnitude -function lj_magnitude(dist, target, epsilon) { - return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) -} - -# Neighbor data to LJ interaction vector -function lj_vector(rid, data) { - return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) -} - -# Accumulator of neighbor LJ interactions -function lj_sum(rid, data, accum) { - return math.vec2.add(data, accum) -} - -# Calculates and actuates the flocking interaction -function hexagon() { - statef=hexagon - CURSTATE = "HEXAGON" - # Calculate accumulator - var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) - if(neighbors.count() > 0) - 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) - -# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS -# timeW =0 -# statef=land -# } else { -# timeW = timeW+1 -# uav_moveto(0.0,0.0) -# } -} - -######################################## -# -# BARRIER-RELATED FUNCTIONS -# -######################################## - -# -# Constants -# -BARRIER_VSTIG = 1 -# ROBOTS = 3 # number of robots in the swarm - -# -# Sets a barrier -# -function barrier_set(threshold, transf) { - statef = function() { - barrier_wait(threshold, transf); - } - barrier = stigmergy.create(BARRIER_VSTIG) -} - -# -# Make yourself ready -# -function barrier_ready() { - barrier.put(id, 1) -} - -# -# Executes the barrier -# -WAIT_TIMEOUT = 200 -timeW=0 -function barrier_wait(threshold, transf) { - barrier.get(id) - CURSTATE = "BARRIERWAIT" - if(barrier.size() >= threshold) { - barrier = nil - transf() - } else if(timeW>=WAIT_TIMEOUT) { - barrier = nil - statef=land - timeW=0 - } - timeW = timeW+1 -} - -# flight status - -function idle() { -statef=idle -CURSTATE = "IDLE" - -} - -function takeoff() { - CURSTATE = "TAKEOFF" - statef=takeoff - log("TakeOff: ", flight.status) - log("Relative position: ", position.altitude) - - if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS,hexagon) - barrier_ready() - #statef=hexagon - } - else { - log("Altitude: ", TARGET_ALTITUDE) - neighbors.broadcast("cmd", 22) - uav_takeoff(TARGET_ALTITUDE) - } -} -function land() { - CURSTATE = "LAND" - statef=land - log("Land: ", flight.status) - if(flight.status == 2 or flight.status == 3){ - neighbors.broadcast("cmd", 21) - uav_land() - } - else { - timeW=0 - barrier = nil - statef=idle - } -} - -# Executed once at init time. -function init() { - s = swarm.create(1) -# s.select(1) - s.join() - statef=idle - CURSTATE = "IDLE" -} - -# Executed at each time step. -function step() { - if(flight.rc_cmd==22) { - log("cmd 22") - flight.rc_cmd=0 - statef = takeoff - CURSTATE = "TAKEOFF" - neighbors.broadcast("cmd", 22) - } else if(flight.rc_cmd==21) { - log("cmd 21") - log("To land") - flight.rc_cmd=0 - statef = land - CURSTATE = "LAND" - neighbors.broadcast("cmd", 21) - } else if(flight.rc_cmd==16) { - flight.rc_cmd=0 - statef = idle - uav_goto() - } else if(flight.rc_cmd==400) { - flight.rc_cmd=0 - uav_arm() - neighbors.broadcast("cmd", 400) - } else if (flight.rc_cmd==401){ - flight.rc_cmd=0 - uav_disarm() - neighbors.broadcast("cmd", 401) - } -neighbors.listen("cmd", - function(vid, value, rid) { - print("Got (", vid, ",", value, ") from robot #", rid) - if(value==22 and CURSTATE=="IDLE") { - statef=takeoff - } else if(value==21) { - statef=land - } else if(value==400 and CURSTATE=="IDLE") { - uav_arm() - } else if(value==401 and CURSTATE=="IDLE"){ - uav_disarm() - } - } - -) - statef() - log("Current state: ", CURSTATE) - log("Swarm size: ",ROBOTS) -} - -# Executed once when the robot (or the simulator) is reset. -function reset() { -} - -# Executed once at the end of experiment. -function destroy() { -} diff --git a/script/update_sim/testflockfev6.bzz b/script/update_sim/testflockfev6.bzz deleted file mode 100644 index 1845eb9..0000000 --- a/script/update_sim/testflockfev6.bzz +++ /dev/null @@ -1,208 +0,0 @@ -# We need this for 2D vectors -# Make sure you pass the correct include path to "bzzc -I ..." -include "/home/ubuntu/buzz/src/include/vec2.bzz" -#################################################################################################### -# 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) -} - -TARGET_ALTITUDE = 3.0 -CURSTATE = "TURNEDOFF" - -# Lennard-Jones parameters -TARGET = 10.0 #0.000001001 -EPSILON = 18.0 #0.001 - -# Lennard-Jones interaction magnitude -function lj_magnitude(dist, target, epsilon) { - return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) -} - -# Neighbor data to LJ interaction vector -function lj_vector(rid, data) { - return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) -} - -# Accumulator of neighbor LJ interactions -function lj_sum(rid, data, accum) { - return math.vec2.add(data, accum) -} - -# Calculates and actuates the flocking interaction -function hexagon() { - statef=hexagon - CURSTATE = "HEXAGON" - # Calculate accumulator - var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) - if(neighbors.count() > 0) - 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) - -# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS -# timeW =0 -# statef=land -# } else { -# timeW = timeW+1 -# uav_moveto(0.0,0.0) -# } -} - -######################################## -# -# BARRIER-RELATED FUNCTIONS -# -######################################## - -# -# Constants -# -BARRIER_VSTIG = 1 -# ROBOTS = 3 # number of robots in the swarm - -# -# Sets a barrier -# -function barrier_set(threshold, transf) { - statef = function() { - barrier_wait(threshold, transf); - } - barrier = stigmergy.create(BARRIER_VSTIG) -} - -# -# Make yourself ready -# -function barrier_ready() { - barrier.put(id, 1) -} - -# -# Executes the barrier -# -WAIT_TIMEOUT = 200 -timeW=0 -function barrier_wait(threshold, transf) { - barrier.get(id) - CURSTATE = "BARRIERWAIT" - if(barrier.size() >= threshold) { - barrier = nil - transf() - } else if(timeW>=WAIT_TIMEOUT) { - barrier = nil - statef=land - timeW=0 - } - timeW = timeW+1 -} - -# flight status - -function idle() { -statef=idle -CURSTATE = "IDLE" - -} - -function takeoff() { - CURSTATE = "TAKEOFF" - statef=takeoff - log("TakeOff: ", flight.status) - log("Relative position: ", position.altitude) - - if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS,hexagon) - barrier_ready() - #statef=hexagon - } - else { - log("Altitude: ", TARGET_ALTITUDE) - neighbors.broadcast("cmd", 22) - uav_takeoff(TARGET_ALTITUDE) - } -} -function land() { - CURSTATE = "LAND" - statef=land - log("Land: ", flight.status) - if(flight.status == 2 or flight.status == 3){ - neighbors.broadcast("cmd", 21) - uav_land() - } - else { - timeW=0 - barrier = nil - statef=idle - } -} - -# Executed once at init time. -function init() { - s = swarm.create(1) -# s.select(1) - s.join() - statef=idle - CURSTATE = "IDLE" -} - -# Executed at each time step. -function step() { - if(flight.rc_cmd==22) { - log("cmd 22") - flight.rc_cmd=0 - statef = takeoff - CURSTATE = "TAKEOFF" - neighbors.broadcast("cmd", 22) - } else if(flight.rc_cmd==21) { - log("cmd 21") - log("To land") - flight.rc_cmd=0 - statef = land - CURSTATE = "LAND" - neighbors.broadcast("cmd", 21) - } else if(flight.rc_cmd==16) { - flight.rc_cmd=0 - statef = idle - uav_goto() - } else if(flight.rc_cmd==400) { - flight.rc_cmd=0 - uav_arm() - neighbors.broadcast("cmd", 400) - } else if (flight.rc_cmd==401){ - flight.rc_cmd=0 - uav_disarm() - neighbors.broadcast("cmd", 401) - } -neighbors.listen("cmd", - function(vid, value, rid) { - print("Got (", vid, ",", value, ") from robot #", rid) - if(value==22 and CURSTATE=="IDLE") { - statef=takeoff - } else if(value==21) { - statef=land - } else if(value==400 and CURSTATE=="IDLE") { - uav_arm() - } else if(value==401 and CURSTATE=="IDLE"){ - uav_disarm() - } - } - -) - statef() - log("Current state: ", CURSTATE) - log("Swarm size: ",ROBOTS) -} - -# Executed once when the robot (or the simulator) is reset. -function reset() { -} - -# Executed once at the end of experiment. -function destroy() { -} diff --git a/script/update_sim/testflockfev7.bzz b/script/update_sim/testflockfev7.bzz deleted file mode 100644 index 1845eb9..0000000 --- a/script/update_sim/testflockfev7.bzz +++ /dev/null @@ -1,208 +0,0 @@ -# We need this for 2D vectors -# Make sure you pass the correct include path to "bzzc -I ..." -include "/home/ubuntu/buzz/src/include/vec2.bzz" -#################################################################################################### -# 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) -} - -TARGET_ALTITUDE = 3.0 -CURSTATE = "TURNEDOFF" - -# Lennard-Jones parameters -TARGET = 10.0 #0.000001001 -EPSILON = 18.0 #0.001 - -# Lennard-Jones interaction magnitude -function lj_magnitude(dist, target, epsilon) { - return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) -} - -# Neighbor data to LJ interaction vector -function lj_vector(rid, data) { - return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) -} - -# Accumulator of neighbor LJ interactions -function lj_sum(rid, data, accum) { - return math.vec2.add(data, accum) -} - -# Calculates and actuates the flocking interaction -function hexagon() { - statef=hexagon - CURSTATE = "HEXAGON" - # Calculate accumulator - var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) - if(neighbors.count() > 0) - 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) - -# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS -# timeW =0 -# statef=land -# } else { -# timeW = timeW+1 -# uav_moveto(0.0,0.0) -# } -} - -######################################## -# -# BARRIER-RELATED FUNCTIONS -# -######################################## - -# -# Constants -# -BARRIER_VSTIG = 1 -# ROBOTS = 3 # number of robots in the swarm - -# -# Sets a barrier -# -function barrier_set(threshold, transf) { - statef = function() { - barrier_wait(threshold, transf); - } - barrier = stigmergy.create(BARRIER_VSTIG) -} - -# -# Make yourself ready -# -function barrier_ready() { - barrier.put(id, 1) -} - -# -# Executes the barrier -# -WAIT_TIMEOUT = 200 -timeW=0 -function barrier_wait(threshold, transf) { - barrier.get(id) - CURSTATE = "BARRIERWAIT" - if(barrier.size() >= threshold) { - barrier = nil - transf() - } else if(timeW>=WAIT_TIMEOUT) { - barrier = nil - statef=land - timeW=0 - } - timeW = timeW+1 -} - -# flight status - -function idle() { -statef=idle -CURSTATE = "IDLE" - -} - -function takeoff() { - CURSTATE = "TAKEOFF" - statef=takeoff - log("TakeOff: ", flight.status) - log("Relative position: ", position.altitude) - - if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS,hexagon) - barrier_ready() - #statef=hexagon - } - else { - log("Altitude: ", TARGET_ALTITUDE) - neighbors.broadcast("cmd", 22) - uav_takeoff(TARGET_ALTITUDE) - } -} -function land() { - CURSTATE = "LAND" - statef=land - log("Land: ", flight.status) - if(flight.status == 2 or flight.status == 3){ - neighbors.broadcast("cmd", 21) - uav_land() - } - else { - timeW=0 - barrier = nil - statef=idle - } -} - -# Executed once at init time. -function init() { - s = swarm.create(1) -# s.select(1) - s.join() - statef=idle - CURSTATE = "IDLE" -} - -# Executed at each time step. -function step() { - if(flight.rc_cmd==22) { - log("cmd 22") - flight.rc_cmd=0 - statef = takeoff - CURSTATE = "TAKEOFF" - neighbors.broadcast("cmd", 22) - } else if(flight.rc_cmd==21) { - log("cmd 21") - log("To land") - flight.rc_cmd=0 - statef = land - CURSTATE = "LAND" - neighbors.broadcast("cmd", 21) - } else if(flight.rc_cmd==16) { - flight.rc_cmd=0 - statef = idle - uav_goto() - } else if(flight.rc_cmd==400) { - flight.rc_cmd=0 - uav_arm() - neighbors.broadcast("cmd", 400) - } else if (flight.rc_cmd==401){ - flight.rc_cmd=0 - uav_disarm() - neighbors.broadcast("cmd", 401) - } -neighbors.listen("cmd", - function(vid, value, rid) { - print("Got (", vid, ",", value, ") from robot #", rid) - if(value==22 and CURSTATE=="IDLE") { - statef=takeoff - } else if(value==21) { - statef=land - } else if(value==400 and CURSTATE=="IDLE") { - uav_arm() - } else if(value==401 and CURSTATE=="IDLE"){ - uav_disarm() - } - } - -) - statef() - log("Current state: ", CURSTATE) - log("Swarm size: ",ROBOTS) -} - -# Executed once when the robot (or the simulator) is reset. -function reset() { -} - -# Executed once at the end of experiment. -function destroy() { -} diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 3a77dba..69d9ec8 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -29,7 +29,7 @@ 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"); + ROS_INFO("intiialized file monitor.\n"); fd=inotify_init1(IN_NONBLOCK); if ( fd < 0 ) { perror( "inotify_init error" ); @@ -48,7 +48,7 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){ BO_BUF = (uint8_t*)malloc(bcode_size); if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { perror(bo_filename); - fclose(fp); + //fclose(fp); //return 0; } fclose(fp); @@ -65,7 +65,7 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){ 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); + //fclose(fp); //return 0; } fclose(fp); @@ -147,7 +147,7 @@ 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); + //ROS_INFO("[DEBUG] Updater append code of 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); @@ -156,9 +156,9 @@ void code_message_inqueue_append(uint8_t* msg,uint16_t 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)) ) ); + ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode) ); + ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) ); + ROS_INFO("[Debug] Updater received code of 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"); @@ -191,7 +191,6 @@ void update_routine(const char* bcfname, const char* dbgfname){ 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); @@ -199,37 +198,13 @@ void update_routine(const char* bcfname, if(*(int*)updater->mode==CODE_RUNNING){ buzzvm_function_call(VM, "updated_neigh", 0); if(check_update()){ - std::string bzzfile_name(bzz_file); - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")); - bzzfile_in_compile<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)); + ROS_INFO("Current 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"); + ROS_INFO("Sending code... \n"); code_message_outqueue_append(); } delete_p(BO_BUF); @@ -268,7 +243,7 @@ void update_routine(const char* bcfname, else{ //gettimeofday(&t1, NULL); if(neigh==0 && (!is_msg_present())){ - fprintf(stdout,"Sending code... \n"); + ROS_INFO("Sending code... \n"); code_message_outqueue_append(); } @@ -277,7 +252,7 @@ void update_routine(const char* bcfname, buzzvm_gload(VM); buzzobj_t tObj = buzzvm_stack_at(VM, 1); buzzvm_pop(VM); - fprintf(stdout,"Barrier ..................... %i \n",tObj->i.value); + ROS_INFO("Barrier ..................... %i \n",tObj->i.value); if(tObj->i.value==no_of_robot) { *(int*)(updater->mode) = CODE_RUNNING; gettimeofday(&t2, NULL); @@ -307,12 +282,12 @@ return (uint8_t*)updater->outmsg_queue->size; int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){ if(buzz_utility::buzz_update_init_test(BO_BUF, dbgfname,bcode_size)){ - fprintf(stdout,"Initializtion of script test passed\n"); + ROS_WARN("Initializtion of script test passed\n"); if(buzz_utility::update_step_test()){ /*data logging*/ //start =1; /*data logging*/ - fprintf(stdout,"Step test passed\n"); + ROS_WARN("Step test passed\n"); *(int*) (updater->mode) = CODE_STANDBY; //fprintf(stdout,"updater value = %i\n",updater->mode); delete_p(updater->bcode); @@ -330,12 +305,12 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){ /*Unable to step something wrong*/ else{ if(*(int*) (updater->mode) == CODE_RUNNING){ - fprintf(stdout,"step test failed, stick to old script\n"); + ROS_ERROR("step test failed, stick to old script\n"); buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t)*(size_t*)(updater->bcode_size)); } else{ /*You will never reach here*/ - fprintf(stdout,"step test failed, Return to stand by\n"); + ROS_ERROR("step test failed, Return to stand by\n"); buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); buzzvm_t VM = buzz_utility::get_vm(); @@ -350,12 +325,12 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){ } else { if(*(int*) (updater->mode) == CODE_RUNNING){ - fprintf(stdout,"Initialization test failed, stick to old script\n"); + ROS_ERROR("Initialization test failed, stick to old script\n"); buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname,(int)*(size_t*) (updater->bcode_size)); } else{ /*You will never reach here*/ - fprintf(stdout,"Initialization test failed, Return to stand by\n"); + ROS_ERROR("Initialization test failed, Return to stand by\n"); buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); buzzvm_t VM = buzz_utility::get_vm(); @@ -420,6 +395,23 @@ void updates_set_robots(int robots){ no_of_robot=robots; } +/*-------------------------------------------------------- +/ Create Buzz bytecode from the bzz script inputed +/-------------------------------------------------------*/ +int compile_bzz(){ + /*Compile the buzz code .bzz to .bo*/ + std::string bzzfile_name(bzz_file); + stringstream bzzfile_in_compile; + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name = name.substr(0,name.find_last_of(".")); + bzzfile_in_compile << "bzzc -I " << path << "include/"; //<<" "<state); + return 0; + } + // Call the Init() function + if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){ + ROS_ERROR("Error in calling init, VM state : %i", VM->state); + return 0; + } /* All OK */ - ROS_INFO("[%i] INIT DONE!!!", Robot_id); + ROS_INFO("[%i] INIT DONE!!!", Robot_id); return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size); } @@ -554,10 +561,17 @@ static int create_stig_tables() { //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; return 0; } + // Execute the global part of the script - buzzvm_execute_script(VM); + if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ + ROS_ERROR("Error executing global part, VM state : %i",VM->state); + return 0; + } // Call the Init() function - buzzvm_function_call(VM, "init", 0); + if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){ + ROS_ERROR("Error in calling init, VM state : %i", VM->state); + return 0; + } // All OK return 1; } @@ -604,9 +618,15 @@ static int create_stig_tables() { return 0; } // Execute the global part of the script - buzzvm_execute_script(VM); + if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ + ROS_ERROR("Error executing global part, VM state : %i",VM->state); + return 0; + } // Call the Init() function - buzzvm_function_call(VM, "init", 0); + if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){ + ROS_ERROR("Error in calling init, VM state : %i", VM->state); + return 0; + } // All OK return 1; } @@ -725,19 +745,25 @@ static int create_stig_tables() { } int update_step_test() { - + /*Process available messages*/ + in_message_process(); 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); + //set_robot_var(buzzdict_size(VM->swarmmembers)+1); int a = buzzvm_function_call(VM, "step", 0); - if(a != BUZZVM_STATE_READY) { + + if(a!= BUZZVM_STATE_READY) { + ROS_ERROR("%s: execution terminated abnormally: %s\n\n", + BO_FNAME, + buzz_error_info()); fprintf(stdout, "step test VM state %i\n",a); - fprintf(stdout, " execution terminated abnormally\n\n"); - } + } + return a == BUZZVM_STATE_READY; } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index b5832c2..2160bb1 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -101,10 +101,11 @@ namespace rosbzz_node{ 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); - /*Set no of robots for updates*/ + /*Set no of robots for updates TODO only when not updating*/ + //if(multi_msg) 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(); From a99b889b163e4bd462c80d29feeaf2895e98f0a0 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 11 May 2017 22:18:50 -0400 Subject: [PATCH 26/26] correction to sim flockfev --- script/update_sim/testflockfev1.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/script/update_sim/testflockfev1.bzz b/script/update_sim/testflockfev1.bzz index e628847..06b3e32 100644 --- a/script/update_sim/testflockfev1.bzz +++ b/script/update_sim/testflockfev1.bzz @@ -26,7 +26,7 @@ function lj_magnitude(dist, target, epsilon) { # Neighbor data to LJ interaction vector function lj_vector(rid, data) { return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) -}cd sr +} # Accumulator of neighbor LJ interactions function lj_sum(rid, data, accum) {