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

merge fix

This commit is contained in:
dave 2017-04-20 23:33:56 -04:00
parent a14b753b7e 3e1653cdaa
commit 0bc4001542
6 changed files with 108 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/buzz_utility.h
View File

@ -49,7 +49,7 @@ int buzz_script_done();
int update_step_test();
uint16_t get_robotid();
int get_robotid();
buzzvm_t get_vm();

3
include/roscontroller.h
View File

@ -70,6 +70,7 @@ private:
//std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
int timer_step=0;
int robot_id=0;
std::string robot_name = "";
//int oldcmdID=0;
int rc_cmd;
float fcu_timeout;
@ -80,7 +81,7 @@ private:
/*tmp to be corrected*/
uint8_t no_cnt=0;
uint8_t old_val=0;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name, robot_name;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name;
std::string stream_client_name;
std::string relative_altitude_sub_name;
std::string setpoint_nonraw;

165
src/buzz_utility.cpp
View File

@ -51,6 +51,9 @@ namespace buzz_utility{
return out;
}
int get_robotid() {
return Robot_id;
}
/***************************************************/
/*Appends obtained messages to buzz in message Queue*/
/***************************************************/
@ -252,18 +255,10 @@ namespace buzz_utility{
int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id) {
//cout<<"bo file name"<<bo_filename;
/* Get hostname */
//char hstnm[30];
//char* hstnm ="M1003";
//gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Mnn */
//int id = strtol(hstnm+4, NULL, 10);
cout << " Robot ID: " <<robot_id<< endl;
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM);
Robot_id =robot_id;
Robot_id = robot_id;
VM = buzzvm_new((int)robot_id);
/* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
@ -287,69 +282,62 @@ namespace buzz_utility{
}
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 */
//if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
// buzzvm_destroy(&VM);
// buzzdebug_destroy(&DBG_INFO);
// fprintf(stdout, "%s: Error loading Buzz script\n\n", bo_filename);
// return 0;
//}
/* Register hook functions */
/*if(buzz_register_hooks() != BUZZVM_STATE_READY) {
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error registering hooks\n\n", bo_filename);
perror(bdbg_filename);
return 0;
}*/
}
/* Set byte code */
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stderr, "%s: Error loading Buzz script\n\n", bo_filename);
return 0;
}
/* Register hook functions */
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stderr, "%s: Error registering hooks\n\n", bo_filename);
return 0;
}
/* Save bytecode file name */
//BO_FNAME = strdup(bo_filename);
BO_FNAME = strdup(bo_filename);
/* Execute the global part of the script */
//buzzvm_execute_script(VM);
buzzvm_execute_script(VM);
/* Call the Init() function */
//buzzvm_function_call(VM, "init", 0);
buzzvm_function_call(VM, "init", 0);
/* All OK */
return buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
/****************************************/
/*Sets a new update */
/****************************************/
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* Get hostname */
//char hstnm[30];
//gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Mnn */
//int id = strtol(hstnm + 4, NULL, 10);
/* Reset the Buzz VM */
/* // Reset the Buzz VM
if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id);
/* Get rid of debug info */
// Get rid of debug info
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
/* Read debug information */
// 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 */
// Set byte code
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
return 0;
}
/* Register hook functions */
// Register hook functions
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
@ -357,46 +345,40 @@ namespace buzz_utility{
return 0;
}
/* Execute the global part of the script */
// Execute the global part of the script
buzzvm_execute_script(VM);
/* Call the Init() function */
// Call the Init() function
buzzvm_function_call(VM, "init", 0);
/* All OK */
return 1;
// All OK
*/ return 1;
}
/****************************************/
/*Performs a initialization test */
/****************************************/
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* Get hostname */
//char hstnm[30];
//gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Mnn */
//int id = strtol(hstnm + 4, NULL, 10);
/* Reset the Buzz VM */
/* // Reset the Buzz VM
if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id);
/* Get rid of debug info */
// Get rid of debug info
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
/* Read debug information */
// 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 */
// Set byte code
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
return 0;
}
/* Register hook functions */
// Register hook functions
if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
@ -404,12 +386,12 @@ namespace buzz_utility{
return 0;
}
/* Execute the global part of the script */
// Execute the global part of the script
buzzvm_execute_script(VM);
/* Call the Init() function */
// Call the Init() function
buzzvm_function_call(VM, "init", 0);
/* All OK */
return 1;
// All OK
*/ return 1;
}
/****************************************/
@ -425,31 +407,35 @@ namespace buzz_utility{
void check_swarm_members(const void* key, void* data, void* params) {
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
int* status = (int*)params;
fprintf(stderr, "CHECKING SWARM MEMBERS\n");
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_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;
}
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;
}
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*/
@ -471,21 +457,16 @@ namespace buzz_utility{
buzz_error_info());
buzzvm_dump(VM);
}
// usleep(10000);
//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);
//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 -- SOMETHING CRASHING HERE!! */
int status = 1;
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
/* if(status == 1 &&
buzzdict_size(VM->swarmmembers) < 9)
status = 2;*/
buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));
buzzvm_pushi(VM, status);
buzzvm_gstore(VM);
/* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
// int status = 1;
// buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
}
/****************************************/

1
src/buzzuav_closures.cpp
View File

@ -30,6 +30,7 @@ namespace buzzuav_closures{
int buzzros_print(buzzvm_t vm) {
int i;
char buffer [50] = "";
sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid());
for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
buzzvm_lload(vm, i);
buzzobj_t o = buzzvm_stack_at(vm, 1);

51
src/roscontroller.cpp
View File

@ -24,12 +24,12 @@ namespace rosbzz_node{
SetStreamRate(0, 10, 1);
/// Get Robot Id - wait for Xbee to be started
if(xbeeplugged)
GetRobotId();
else
robot_id = strtol(robot_name.c_str()+4, NULL, 10);
GetRobotId();
else
robot_id=strtol(robot_name.c_str() + 5, NULL, 10);;
setpoint_counter = 0;
fcu_timeout = TIMEOUT;
home[0]=0;home[1]=0;home[2]=0;
home[0]=0.0;home[1]=0.0;home[2]=0.0;
}
/*---------------------
@ -46,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)){
@ -54,6 +55,8 @@ namespace rosbzz_node{
}
robot_id=robot_id_srv_response.value.integer;
//robot_id = 8;
}
/*-------------------------------------------------
@ -64,7 +67,10 @@ 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*/
@ -72,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*/
@ -80,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();
@ -143,9 +149,11 @@ namespace rosbzz_node{
n_c.getParam("stand_by", stand_by);
if(n_c.getParam("xbee_plugged", xbeeplugged));
else {ROS_ERROR("Provide the xbee_plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");}
n_c.getParam("xbee_status_srv", xbeesrv_name);
n_c.getParam("robot_name", robot_name);
else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");}
if(!xbeeplugged){
if(n_c.getParam("robot_name", robot_name));
else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");}
}
GetSubscriptionParameters(n_c);
// initialize topics to null?
@ -158,7 +166,8 @@ namespace rosbzz_node{
//todo: make it as an array in yaml?
m_sMySubscriptions.clear();
std::string gps_topic, gps_type;
node_handle.getParam("topics/gps", gps_topic);
if(node_handle.getParam("topics/gps", gps_topic));
else {ROS_ERROR("Provide a gps topic in Launch file"); system("rosnode kill rosbuzz_node");}
node_handle.getParam("type/gps", gps_type);
m_smTopic_infos.insert(pair <std::string, std::string>(gps_topic, gps_type));
@ -200,7 +209,7 @@ namespace rosbzz_node{
//current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this);
//battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this);
payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this);
payload_sub = n_c.subscribe("/"+robot_name+in_payload, 1000, &roscontroller::payload_obt,this);
//flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this);
//Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this);
obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
@ -346,35 +355,35 @@ 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;
mavros_msgs::Mavlink update_packets;
fprintf(stdout,"Transfering code \n");
fprintf(stdout,"Sent Update packet Size: %u \n",updater_msgSize);
/*allocate mem and clear it*/
// allocate mem and clear it
buff_send =(uint8_t*)malloc(sizeof(uint16_t)+updater_msgSize);
memset(buff_send, 0,sizeof(uint16_t)+updater_msgSize);
/*Append updater msg size*/
// Append updater msg size
*(uint16_t*)(buff_send + tot)=updater_msgSize;
//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
tot += sizeof(uint16_t);
/*Append updater msgs*/
// Append updater msgs
memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
tot += updater_msgSize;
/*Destroy the updater out msg queue*/
// Destroy the updater out msg queue
destroy_out_msg_queue();
uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t)));
uint64_t* payload_64 = new uint64_t[total_size];
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
free(buff_send);
/*Send a constant number to differenciate updater msgs*/
// Send a constant number to differenciate updater msgs
update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT);
for(int i=0;i<total_size;i++){
update_packets.payload64.push_back(payload_64[i]);
}
/*Add Robot id and message number to the published message*/
// Add Robot id and message number to the published message
if (message_number < 0) message_number=0;
else message_number++;
update_packets.sysid=(uint8_t)robot_id;
@ -382,7 +391,7 @@ namespace rosbzz_node{
payload_pub.publish(update_packets);
multi_msg=false;
delete[] payload_64;
}
}*/
}
/*---------------------------------------------------------------------------------

2
src/testflockfev.bzz
View File

@ -50,7 +50,7 @@ function hexagon() {
statef=land
} else {
timeW = timeW+1
uav_moveto(0.0,0.35)
uav_moveto(0.0,0.0)
}
}