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

merged and fix first vstig

This commit is contained in:
dave 2017-05-07 00:41:20 -04:00
parent 5070268491 a921c3819c
commit a38ce4f010
8 changed files with 424 additions and 490 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/buzz_utility.h
View File

@ -34,19 +34,17 @@ uint16_t* u64_cvt_u16(uint64_t u64);
int buzz_listen(const char* type,
int msg_size);
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map);
void 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);
void in_msg_append(uint64_t* payload);
void in_msg_process(uint64_t* payload);
uint64_t* obt_out_msg();
uint64_t* out_msg_process();
void update_sensors();
int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id);

4
include/buzzuav_closures.h
View File

@ -51,7 +51,9 @@ void set_userspos(double latitude, double longitude, double altitude);
double* getgoto();
/* updates flight status*/
void flight_status_update(uint8_t state);
/* Update neighbors table */
void neighbour_pos_callback(int id, float range, float bearing, float elevation);
void update_neighbors(buzzvm_t vm);
/*Flight status*/
void set_obstacle_dist(float dist[]);

12
launch/rosbuzzm100.launch
View File

@ -2,16 +2,18 @@
<launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/test1.bzz" />
<rosparam file="$(find rosbuzz)/launch/launch_config/m100.yaml"/>
<param name="bzzfile_name" value="$(env ROS_WS)/src/testflockfev.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<param name="fcclient_name" value="/dji_mavcmd" />
<!--param name="fcclient_name" value="/dji_mavcmd" -->
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="xbee_status_srv" value="/xbee_status"/>
<param name="robot_id" value="3"/>
<param name="No_of_Robots" value="3"/>
<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
<param name="xbee_plugged" value="true"/>
<!--param name="robot_id" value="3"-->
<!--param name="No_of_Robots" value="3"-->
<param name="stand_by" value="$(env ROS_WS)/src/stand_by.bo"/>
</node>

361
src/buzz_update.cpp
View File

@ -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(i<len){
event=(struct inotify_event *) &buf[i];
/* file was modified this flag is true in nano and self delet in gedit and other editors */
//fprintf(stdout,"inside file monitor.\n");
if(event->mask & (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 "<<bzzfile_name<<" "<<path<< name<<".basm";
FILE *fp;
int comp=0;
char buf[128];
fprintf(stdout,"Update found \nUpdating script ...\n");
if ((fp = popen(bzzfile_in_compile.str().c_str(), "r")) == NULL) { // to change file edit
fprintf(stdout,"Error opening pipe!\n");
}
while (fgets(buf, 128, fp) != NULL) {
fprintf(stdout,"OUTPUT: %s \n", buf);
comp=1;
}
}
while (fgets(buf, 128, fp) != NULL) {
fprintf(stdout,"OUTPUT: %s \n", buf);
comp=1;
}
bzzfile_in_compile.str("");
bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
if ((fp = popen(bzzfile_in_compile.str().c_str(), "r")) == NULL) { // to change file edit
fprintf(stdout,"Error opening pipe!\n");
}
while (fgets(buf, 128, fp) != NULL) {
fprintf(stdout,"Error opening pipe!\n");
}
while (fgets(buf, 128, fp) != NULL) {
fprintf(stdout,"OUTPUT: %s \n", buf);
}
if(pclose(fp) || comp) {
fprintf(stdout,"Errors in comipilg script so staying on old script\n");
// return -1;
}
else {
}
if(pclose(fp) || comp) {
fprintf(stdout,"Errors in comipilg script so staying on old script\n");
}
else {
uint8_t* BO_BUF = 0;
FILE* fp = fopen(bcfname, "rb"); // to change file edit
if(!fp) {
perror(bcfname);
}
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(bcfname);
fclose(fp);
}
fclose(fp);
if(!fp) {
perror(bcfname);
}
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(bcfname);
fclose(fp);
}
fclose(fp);
if(test_set_code(BO_BUF, dbgfname,bcode_size)){
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();
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);
}

295
src/buzz_utility.cpp
View File

@ -22,32 +22,8 @@ namespace buzz_utility{
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);
/* Get robot id and update neighbor information */
map< int, Pos_struct >::iterator it;
for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){
buzzneighbors_add(VM,
it->first,
(it->second).x,
(it->second).y,
(it->second).z);
}
}
void add_user(int id, double latitude, double longitude, float altitude)
{
@ -58,8 +34,8 @@ namespace buzz_utility{
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));
//ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
}
void update_users(){
@ -68,6 +44,7 @@ namespace buzz_utility{
/* 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,
@ -103,11 +80,12 @@ namespace buzz_utility{
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_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);
//buzzvm_dump(VM);
/* Get "data" field */
buzzvm_pushs(VM, buzzvm_string_register(VM, "data", 1));
buzzvm_tget(VM);
@ -144,6 +122,8 @@ namespace buzz_utility{
/* Save entry into data table */
buzzvm_push(VM, entry);
buzzvm_tput(VM);
ROS_INFO("Buzz_utility saved new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
//buzzvm_dump(VM);
return VM->state;
}
/**************************************************************************/
@ -176,7 +156,7 @@ namespace buzz_utility{
/*|__________________________________________________________________________|____________________________________|*/
/*******************************************************************************************************************/
void in_msg_process(uint64_t* payload){
void in_msg_append(uint64_t* payload){
/* Go through messages and add them to the FIFO */
uint16_t* data= u64_cvt_u16((uint64_t)payload[0]);
@ -205,15 +185,14 @@ namespace buzz_utility{
}
}while(size - tot > 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*/
@ -259,8 +238,8 @@ namespace buzz_utility{
cout<<" payload from out msg "<<*(payload_64+i)<<endl;
}*/
/* Send message */
return payload_64;
}
return payload_64;
}
/****************************************/
/*Buzz script not able to load*/
@ -285,7 +264,7 @@ namespace buzz_utility{
VM->pc,
VM->errormsg);
}
return msg;
return msg;
}
/****************************************/
@ -405,40 +384,39 @@ static int create_stig_tables() {
/****************************************/
/*Sets the .bzz and .bdbg file*/
/****************************************/
int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id) {
ROS_INFO(" Robot ID: %i", robot_id);
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM);
Robot_id = robot_id;
VM = buzzvm_new((int)robot_id);
/* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
/* Read bytecode and fill in data structure */
FILE* fd = fopen(bo_filename, "rb");
if(!fd) {
perror(bo_filename);
ROS_INFO(" Robot ID: " , robot_id);
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM);
Robot_id = robot_id;
VM = buzzvm_new((int)robot_id);
/* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
/* Read bytecode and fill in data structure */
FILE* fd = fopen(bo_filename, "rb");
if(!fd) {
perror(bo_filename);
return 0;
}
fseek(fd, 0, SEEK_END);
size_t bcode_size = ftell(fd);
rewind(fd);
BO_BUF = (uint8_t*)malloc(bcode_size);
if(fread(BO_BUF, 1, bcode_size, fd) < bcode_size) {
perror(bo_filename);
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fclose(fd);
}
fseek(fd, 0, SEEK_END);
size_t bcode_size = ftell(fd);
rewind(fd);
BO_BUF = (uint8_t*)malloc(bcode_size);
if(fread(BO_BUF, 1, bcode_size, fd) < bcode_size) {
perror(bo_filename);
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fclose(fd);
return 0;
}
fclose(fd);
/* Read debug information */
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename);
}
fclose(fd);
/* Read debug information */
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename);
return 0;
}
/* Set byte code */
@ -478,47 +456,11 @@ static int create_stig_tables() {
return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
/****************************************/
/*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, 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);*/
// 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 */
/****************************************/
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* // Reset the Buzz VM
// Reset the Buzz VM
if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id);
// Get rid of debug info
@ -552,14 +494,14 @@ static int create_stig_tables() {
// Call the Init() function
buzzvm_function_call(VM, "init", 0);
// All OK
*/ return 1;
return 1;
}
/****************************************/
/*Performs a initialization test */
/****************************************/
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* // Reset the Buzz VM
// Reset the Buzz VM
if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id);
// Get rid of debug info
@ -593,7 +535,7 @@ static int create_stig_tables() {
// Call the Init() function
buzzvm_function_call(VM, "init", 0);
// All OK
*/ return 1;
return 1;
}
/****************************************/
@ -607,75 +549,75 @@ static int create_stig_tables() {
typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
void check_swarm_members(const void* key, void* data, void* params) {
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
int* status = (int*)params;
if(*status == 3) return;
fprintf(stderr, "CHECKING SWARM MEMBERS:%i\n",buzzdarray_get(e->swarms, 0, uint16_t));
if(buzzdarray_size(e->swarms) != 1) {
fprintf(stderr, "Swarm list size is not 1\n");
*status = 3;
}
else {
int sid = 1;
if(!buzzdict_isempty(VM->swarms)) {
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
sid,
buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
}
if(buzzdict_size(VM->swarms)>1) {
sid = 2;
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
sid,
buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
}
}
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
int* status = (int*)params;
if(*status == 3) return;
fprintf(stderr, "CHECKING SWARM :%i, member: %i, age: %i \n",
buzzdarray_get(e->swarms, 0, uint16_t), *(uint16_t*)key, e->age);
if(buzzdarray_size(e->swarms) != 1) {
fprintf(stderr, "Swarm list size is not 1\n");
*status = 3;
}
else {
int sid = 1;
if(!buzzdict_isempty(VM->swarms)) {
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
sid,
buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
}
if(buzzdict_size(VM->swarms)>1) {
sid = 2;
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
sid,
buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
}
}
}
/*Step through the buzz script*/
void update_sensors(){
/* Update sensors*/
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::update_neighbors(VM);
update_users();
buzzuav_closures::buzzuav_update_userspos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
}
void buzz_script_step() {
/*
* Update sensors
*/
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
update_neighbors();
update_users();
buzzuav_closures::buzzuav_update_userspos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
//buzz_update_users_stigmergy(tbl);
/*
* Call Buzz step() function
*/
if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
/* Process messages */
buzzvm_process_inmsgs(VM);
/*Update sensors*/
update_sensors();
/* Call Buzz step() function */
if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
BO_FNAME,
buzz_error_info());
buzzvm_dump(VM);
}
//buzzvm_process_outmsgs(VM); //--> done in out_msg_process() function called each step
//usleep(10000);
/*Print swarm*/
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
//int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
//fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
buzzvm_dump(VM);
}
/* Process out messages */
buzzvm_process_outmsgs(VM);
/*Print swarm*/
buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
//int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
//fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
/* 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);
}
/****************************************/
@ -725,26 +667,15 @@ static int create_stig_tables() {
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);
}
}

30
src/buzzuav_closures.cpp
View File

@ -25,6 +25,10 @@ namespace buzzuav_closures{
static int rc_cmd=0;
static int buzz_cmd=0;
static float height=0;
std::map< int, buzz_utility::Pos_struct> neighbors_map;
/****************************************/
/****************************************/
@ -245,6 +249,32 @@ namespace buzzuav_closures{
cur_pos[1]=longitude;
cur_pos[2]=altitude;
}
/*adds neighbours position*/
void neighbour_pos_callback(int id, float range, float bearing, float elevation){
buzz_utility::Pos_struct pos_arr;
pos_arr.x=range;
pos_arr.y=bearing;
pos_arr.z=elevation;
map< int, buzz_utility::Pos_struct >::iterator it = neighbors_map.find(id);
if(it!=neighbors_map.end())
neighbors_map.erase(it);
neighbors_map.insert(make_pair(id, pos_arr));
}
/* update at each step the VM table */
void update_neighbors(buzzvm_t vm){
/* Reset neighbor information */
buzzneighbors_reset(vm);
/* Get robot id and update neighbor information */
map< int, buzz_utility::Pos_struct >::iterator it;
for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){
buzzneighbors_add(vm,
it->first,
(it->second).x,
(it->second).y,
(it->second).z);
}
}
void set_userspos(double latitude, double longitude, double altitude){
/*buzz_utility::Pos_struct pos_arr;
pos_arr.x=latitude;

188
src/roscontroller.cpp
View File

@ -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;
@ -68,18 +68,18 @@ 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
//////////////////////////////////////////////////////
while (ros::ok() && !buzz_utility::buzz_script_done())
{
/*Update neighbors position inside Buzz*/
buzz_utility::neighbour_pos_callback(neighbours_pos_map);
//buzz_closure::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());
update_routine(bcfname.c_str(), dbgfname.c_str());
/*Step buzz script */
buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/
@ -87,10 +87,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();
@ -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?
@ -267,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 "<<bzzfile_name<<" "<<path<< name<<".basm";
system(bzzfile_in_compile.str().c_str());
bzzfile_in_compile.str("");
bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
bzzfile_in_compile << "bzzc "<<bzzfile_name; //<<" "<<path<< name<<".basm";
system(bzzfile_in_compile.str().c_str());
//bzzfile_in_compile.str("");
//bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
//system(bzzfile_in_compile.str().c_str());
bzzfile_in_compile.str("");
bzzfile_in_compile <<path<<name<<".bo";
bcfname = bzzfile_in_compile.str();
bzzfile_in_compile.str("");
bzzfile_in_compile <<path<<name<<".bdbg";
bzzfile_in_compile <<path<<name<<".bdb";
dbgfname = bzzfile_in_compile.str();
}
@ -333,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));
@ -357,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;
@ -393,7 +394,7 @@ namespace rosbzz_node{
payload_pub.publish(update_packets);
multi_msg=false;
delete[] payload_64;
}*/
}
}
/*---------------------------------------------------------------------------------
@ -557,95 +558,92 @@ namespace rosbzz_node{
void roscontroller::cvt_rangebearing_coordinates(double nei[], double out[], double cur[]){
// calculate earth radii
double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)));
double prime_vertical_radius = equatorial_radius * sqrt(temp);
double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));
// calculate earth radii
/*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)));
double prime_vertical_radius = equatorial_radius * sqrt(temp);
double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));*/
/*double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
double ned[3];
ned[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS;
ned[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS
double ecef[3];
double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2];
double d = WGS84_E * sin(llh[0]);
double N = WGS84_A / sqrt(1. - d*d);
ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double ref_ecef[3];
llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2];
d = WGS84_E * sin(llh[0]);
N = WGS84_A / sqrt(1. - d*d);
ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double M[3][3];
ecef2ned_matrix(ref_ecef, M);
double ned[3];
matrix_multiply(3, 3, 1, (double *)M, ecef, ned);
out[0] = sqrt(ned[0]*ned[0]+ned[1]*ned[1]);
out[0] = std::floor(out[0] * 1000000) / 1000000;
double d_lat = nei[0] - cur[0];
double ned[3];
ned[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS;
ned[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS
double ecef[3];
double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2];
double d = WGS84_E * sin(llh[0]);
double N = WGS84_A / sqrt(1. - d*d);
ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double ref_ecef[3];
llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2];
d = WGS84_E * sin(llh[0]);
N = WGS84_A / sqrt(1. - d*d);
ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double M[3][3];
ecef2ned_matrix(ref_ecef, M);
double ned[3];
matrix_multiply(3, 3, 1, (double *)M, ecef, ned);
out[0] = sqrt(ned[0]*ned[0]+ned[1]*ned[1]);
out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = atan2(ned[1],ned[0]);
out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0;*/
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
//out[0] = std::floor(out[0] * 1000000) / 1000000;
//out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = atan2(ned_y,ned_x);
//out[1] = std::floor(out[1] * 1000000) / 1000000;
//out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0;
}
void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){
// calculate earth radii
/*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)));
double prime_vertical_radius = equatorial_radius * sqrt(temp);
double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
out[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS;
out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS
out[1] = std::floor(out[1] * 1000000) / 1000000;
// calculate earth radii
/*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)));
double prime_vertical_radius = equatorial_radius * sqrt(temp);
double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
out[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS;
out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS
out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = cur[2];
// Using functions of the library Swift Nav (https://github.com/swift-nav/libswiftnav)
double ecef[3];
double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2];
double d = WGS84_E * sin(llh[0]);
double N = WGS84_A / sqrt(1. - d*d);
ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double ref_ecef[3];
llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2];
d = WGS84_E * sin(llh[0]);
N = WGS84_A / sqrt(1. - d*d);
ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double M[3][3];
ecef2ned_matrix(ref_ecef, M);
matrix_multiply(3, 3, 1, (double *)M, ecef, out);*/
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
//out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
//out[1] = std::floor(out[1] * 1000000) / 1000000;
// Using functions of the library Swift Nav (https://github.com/swift-nav/libswiftnav)
double ecef[3];
double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2];
double d = WGS84_E * sin(llh[0]);
double N = WGS84_A / sqrt(1. - d*d);
ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double ref_ecef[3];
llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2];
d = WGS84_E * sin(llh[0]);
N = WGS84_A / sqrt(1. - d*d);
ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double M[3][3];
ecef2ned_matrix(ref_ecef, M);
matrix_multiply(3, 3, 1, (double *)M, ecef, out);*/
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
//out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
//out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0;
}
@ -852,10 +850,12 @@ namespace rosbzz_node{
/*pass neighbour position to local maintaner*/
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
/*Put RID and pos*/
raw_neighbours_pos_put((int)out[1],raw_neigh_pos);
raw_neighbours_pos_put((int)out[1],raw_neigh_pos);
/* TODO: remove roscontroller local map array for neighbors */
neighbours_pos_put((int)out[1],n_pos);
buzzuav_closures::neighbour_pos_callback((int)out[1],n_pos.x,n_pos.y,n_pos.z);
delete[] out;
buzz_utility::in_msg_process((message_obt+3));
buzz_utility::in_msg_append((message_obt+3));
}
}

16
src/testflockfev.bzz
View File

@ -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)
# }
}
########################################