merged and fix first vstig
This commit is contained in:
dave
commit
a38ce4f010
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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[]);
|
||||
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -27,58 +27,51 @@ 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 */
|
||||
/* 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);
|
||||
/*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);
|
||||
|
||||
}
|
||||
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));
|
||||
/* Create a new table for updater*/
|
||||
/*Intialize the updater with the required data*/
|
||||
updater->bcode = BO_BUF;
|
||||
updater->outmsg_queue = NULL;
|
||||
updater->inmsg_queue = NULL;
|
||||
|
|
@ -98,9 +91,8 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){
|
|||
// 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,19 +114,11 @@ 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;
|
||||
}
|
||||
|
||||
|
|
@ -169,7 +152,6 @@ 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(){
|
||||
|
|
@ -226,41 +208,33 @@ 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;
|
||||
}
|
||||
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,"OUTPUT: %s \n", buf);
|
||||
}
|
||||
|
||||
if(pclose(fp) || comp) {
|
||||
fprintf(stdout,"Errors in comipilg script so staying on old script\n");
|
||||
// return -1;
|
||||
}
|
||||
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);
|
||||
|
|
@ -268,9 +242,7 @@ buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
|
|||
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)){
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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*/
|
||||
|
|
@ -405,10 +384,9 @@ 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);
|
||||
ROS_INFO(" Robot ID: " , robot_id);
|
||||
/* Reset the Buzz VM */
|
||||
if(VM) buzzvm_destroy(&VM);
|
||||
Robot_id = robot_id;
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
|
|
@ -610,7 +552,8 @@ static int create_stig_tables() {
|
|||
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));
|
||||
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;
|
||||
|
|
@ -640,42 +583,41 @@ static int create_stig_tables() {
|
|||
}
|
||||
}
|
||||
}
|
||||
/*Step through the buzz script*/
|
||||
|
||||
void buzz_script_step() {
|
||||
/*
|
||||
* Update sensors
|
||||
*/
|
||||
/*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);
|
||||
update_neighbors();
|
||||
buzzuav_closures::update_neighbors(VM);
|
||||
update_users();
|
||||
buzzuav_closures::buzzuav_update_userspos(VM);
|
||||
buzzuav_closures::buzzuav_update_flight_status(VM);
|
||||
}
|
||||
|
||||
//buzz_update_users_stigmergy(tbl);
|
||||
|
||||
|
||||
/*
|
||||
* Call Buzz step() function
|
||||
*/
|
||||
void buzz_script_step() {
|
||||
/* 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);
|
||||
/* 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);
|
||||
|
||||
/* 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);
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
|
|
@ -725,17 +667,6 @@ 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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}*/
|
||||
}
|
||||
|
||||
}
|
||||
/*---------------------------------------------------------------------------------
|
||||
|
|
@ -558,10 +559,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 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 radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));*/
|
||||
|
||||
/*double d_lon = nei[1] - cur[1];
|
||||
double d_lat = nei[0] - cur[0];
|
||||
|
|
@ -602,9 +603,6 @@ namespace rosbzz_node{
|
|||
out[1] = atan2(ned_y,ned_x);
|
||||
//out[1] = std::floor(out[1] * 1000000) / 1000000;
|
||||
out[2] = 0.0;
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){
|
||||
|
|
@ -853,9 +851,11 @@ namespace rosbzz_node{
|
|||
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);
|
||||
/* 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));
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
# }
|
||||
}
|
||||
|
||||
########################################
|
||||
|
|
|
|||
Loading…
Reference in New Issue