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

swarm table test

This commit is contained in:
dave 2017-04-15 18:17:19 -04:00
parent 2cafc1cbe1
commit 73c81fb22b
3 changed files with 107 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
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;

162
src/buzz_utility.cpp
View File

@ -252,18 +252,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 +279,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 +342,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 +383,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 +404,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 +454,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);
}
/****************************************/

60
src/roscontroller.cpp
View File

@ -23,7 +23,10 @@ namespace rosbzz_node{
// set stream rate - wait for the FC to be started
SetStreamRate(0, 10, 1);
/// Get Robot Id - wait for Xbee to be started
GetRobotId();
if(xbeeplugged)
GetRobotId();
else
robot_id=strtol(robot_name.c_str() + 5, NULL, 10);;
setpoint_counter = 0;
fcu_timeout = TIMEOUT;
}
@ -63,7 +66,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*/
@ -71,7 +77,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*/
@ -79,10 +85,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,6 +149,10 @@ namespace rosbzz_node{
n_c.getParam("xbee_status_srv", xbeesrv_name);
if(n_c.getParam("xbee_plugged", xbeeplugged));
else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_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");}
}
GetSubscriptionParameters(n_c);
// initialize topics to null?
@ -205,13 +215,13 @@ namespace rosbzz_node{
/*publishers*/
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
localsetpoint_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name,1000);
localsetpoint_pub = n_c.advertise<geometry_msgs::PoseStamped>("/"+robot_name+setpoint_name,1000);
/* Service Clients*/
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>("/"+robot_name+armclient);
mode_client = n_c.serviceClient<mavros_msgs::SetMode>("/"+robot_name+modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>("/"+robot_name+fcclient_name);
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>("/"+robot_name+xbeesrv_name);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>("/"+robot_name+stream_client_name);
multi_msg=true;
}
@ -222,19 +232,19 @@ namespace rosbzz_node{
for(std::map<std::string, std::string>::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it){
if(it->second == "mavros_msgs/ExtendedState"){
flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this);
flight_status_sub = n_c.subscribe("/"+robot_name+it->first, 100, &roscontroller::flight_extended_status_update, this);
}
else if(it->second == "mavros_msgs/State"){
flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this);
flight_status_sub = n_c.subscribe("/"+robot_name+it->first, 100, &roscontroller::flight_status_update, this);
}
else if(it->second == "mavros_msgs/BatteryStatus"){
battery_sub = n_c.subscribe(it->first, 1000, &roscontroller::battery, this);
battery_sub = n_c.subscribe("/"+robot_name+it->first, 1000, &roscontroller::battery, this);
}
else if(it->second == "sensor_msgs/NavSatFix"){
current_position_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this);
current_position_sub = n_c.subscribe("/"+robot_name+it->first, 1000, &roscontroller::current_pos, this);
}
else if(it->second == "std_msgs/Float64"){
relative_altitude_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_rel_alt, this);
relative_altitude_sub = n_c.subscribe("/"+robot_name+it->first, 1000, &roscontroller::current_rel_alt, this);
}
std::cout << "Subscribed to: " << it->first << endl;
@ -344,35 +354,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;
@ -380,7 +390,7 @@ namespace rosbzz_node{
payload_pub.publish(update_packets);
multi_msg=false;
delete[] payload_64;
}
}*/
}
/*---------------------------------------------------------------------------------
@ -807,7 +817,7 @@ namespace rosbzz_node{
void roscontroller::SetMode(std::string mode, int delay_miliseconds){
// wait if necessary
if (delay_miliseconds != 0){
std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) );
ros::Duration(delay_miliseconds/1000).sleep();
}
// set mode
mavros_msgs::SetMode set_mode_message;