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; //std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
int timer_step=0; int timer_step=0;
int robot_id=0; int robot_id=0;
std::string robot_name = "";
//int oldcmdID=0; //int oldcmdID=0;
int rc_cmd; int rc_cmd;
float fcu_timeout; 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, int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id) { 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; cout << " Robot ID: " <<robot_id<< endl;
/* Reset the Buzz VM */ /* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM); if(VM) buzzvm_destroy(&VM);
Robot_id =robot_id; Robot_id = robot_id;
VM = buzzvm_new((int)robot_id); VM = buzzvm_new((int)robot_id);
/* Get rid of debug info */ /* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
@ -287,69 +279,62 @@ namespace buzz_utility{
} }
fclose(fd); fclose(fd);
/* Read debug information */ /* Read debug information */
//if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) { 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) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error registering hooks\n\n", bo_filename); perror(bdbg_filename);
return 0; 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 */ /* Save bytecode file name */
//BO_FNAME = strdup(bo_filename); BO_FNAME = strdup(bo_filename);
/* Execute the global part of the script */ /* Execute the global part of the script */
//buzzvm_execute_script(VM); buzzvm_execute_script(VM);
/* Call the Init() function */ /* Call the Init() function */
//buzzvm_function_call(VM, "init", 0); buzzvm_function_call(VM, "init", 0);
/* All OK */ /* 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 */ /*Sets a new update */
/****************************************/ /****************************************/
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* Get hostname */ /* // Reset the Buzz VM
//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 */
if(VM) buzzvm_destroy(&VM); if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id); VM = buzzvm_new(Robot_id);
/* Get rid of debug info */ // Get rid of debug info
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new(); DBG_INFO = buzzdebug_new();
/* Read debug information */ // Read debug information
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) { if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename); perror(bdbg_filename);
return 0; return 0;
} }
/* Set byte code */ // Set byte code
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME); fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
return 0; return 0;
} }
/* Register hook functions */ // Register hook functions
if(buzz_register_hooks() != BUZZVM_STATE_READY) { if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
@ -357,46 +342,40 @@ namespace buzz_utility{
return 0; return 0;
} }
/* Execute the global part of the script */ // Execute the global part of the script
buzzvm_execute_script(VM); buzzvm_execute_script(VM);
/* Call the Init() function */ // Call the Init() function
buzzvm_function_call(VM, "init", 0); buzzvm_function_call(VM, "init", 0);
/* All OK */ // All OK
return 1; */ return 1;
} }
/****************************************/ /****************************************/
/*Performs a initialization test */ /*Performs a initialization test */
/****************************************/ /****************************************/
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){ int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* Get hostname */ /* // Reset the Buzz VM
//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 */
if(VM) buzzvm_destroy(&VM); if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id); VM = buzzvm_new(Robot_id);
/* Get rid of debug info */ // Get rid of debug info
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new(); DBG_INFO = buzzdebug_new();
/* Read debug information */ // Read debug information
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) { if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename); perror(bdbg_filename);
return 0; return 0;
} }
/* Set byte code */ // Set byte code
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME); fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
return 0; return 0;
} }
/* Register hook functions */ // Register hook functions
if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) { if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
@ -404,12 +383,12 @@ namespace buzz_utility{
return 0; return 0;
} }
/* Execute the global part of the script */ // Execute the global part of the script
buzzvm_execute_script(VM); buzzvm_execute_script(VM);
/* Call the Init() function */ // Call the Init() function
buzzvm_function_call(VM, "init", 0); buzzvm_function_call(VM, "init", 0);
/* All OK */ // All OK
return 1; */ return 1;
} }
/****************************************/ /****************************************/
@ -425,31 +404,35 @@ namespace buzz_utility{
void check_swarm_members(const void* key, void* data, void* params) { void check_swarm_members(const void* key, void* data, void* params) {
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data; buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
int* status = (int*)params; int* status = (int*)params;
fprintf(stderr, "CHECKING SWARM MEMBERS\n");
if(*status == 3) return; if(*status == 3) return;
fprintf(stderr, "CHECKING SWARM MEMBERS:%i\n",buzzdarray_get(e->swarms, 0, uint16_t));
if(buzzdarray_size(e->swarms) != 1) { if(buzzdarray_size(e->swarms) != 1) {
fprintf(stderr, "Swarm list size is not 1\n"); fprintf(stderr, "Swarm list size is not 1\n");
*status = 3; *status = 3;
} }
else { else {
int sid = 1; int sid = 1;
if(*buzzdict_get(VM->swarms, &sid, uint8_t) && if(!buzzdict_isempty(VM->swarms)) {
buzzdarray_get(e->swarms, 0, uint16_t) != sid) { if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
sid, fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
buzzdarray_get(e->swarms, 0, uint16_t)); sid,
*status = 3; buzzdarray_get(e->swarms, 0, uint16_t));
return; *status = 3;
} return;
sid = 2; }
if(*buzzdict_get(VM->swarms, &sid, uint8_t) && }
buzzdarray_get(e->swarms, 0, uint16_t) != sid) { if(buzzdict_size(VM->swarms)>1) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", sid = 2;
sid, if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t)); buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
*status = 3; fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
return; sid,
} buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
}
} }
} }
/*Step through the buzz script*/ /*Step through the buzz script*/
@ -471,21 +454,16 @@ namespace buzz_utility{
buzz_error_info()); buzz_error_info());
buzzvm_dump(VM); buzzvm_dump(VM);
} }
// usleep(10000); //buzzvm_process_outmsgs(VM); //--> done in out_msg_process() function called each step
//usleep(10000);
/*Print swarm*/ /*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; //int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize); //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
/* Check swarm state -- SOMETHING CRASHING HERE!! */ /* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
int status = 1; // int status = 1;
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status); // 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);
} }
/****************************************/ /****************************************/

62
src/roscontroller.cpp
View File

@ -23,7 +23,10 @@ namespace rosbzz_node{
// set stream rate - wait for the FC to be started // set stream rate - wait for the FC to be started
SetStreamRate(0, 10, 1); SetStreamRate(0, 10, 1);
/// Get Robot Id - wait for Xbee to be started /// 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; setpoint_counter = 0;
fcu_timeout = TIMEOUT; fcu_timeout = TIMEOUT;
} }
@ -63,7 +66,10 @@ namespace rosbzz_node{
/* Set the Buzz bytecode */ /* Set the Buzz bytecode */
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) { if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
fprintf(stdout, "Bytecode file found and set\n"); 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()) while (ros::ok() && !buzz_utility::buzz_script_done())
{ {
/*Update neighbors position inside Buzz*/ /*Update neighbors position inside Buzz*/
@ -71,7 +77,7 @@ namespace rosbzz_node{
/*Neighbours of the robot published with id in respective topic*/ /*Neighbours of the robot published with id in respective topic*/
neighbours_pos_publisher(); neighbours_pos_publisher();
/*Check updater state and step code*/ /*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 */ /*Step buzz script */
buzz_utility::buzz_script_step(); buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/ /*Prepare messages and publish them in respective topic*/
@ -79,10 +85,10 @@ namespace rosbzz_node{
/*call flight controler service to set command long*/ /*call flight controler service to set command long*/
flight_controller_service_call(); flight_controller_service_call();
/*Set multi message available after update*/ /*Set multi message available after update*/
if(get_update_status()){ /*if(get_update_status()){
set_read_update_status(); set_read_update_status();
multi_msg=true; multi_msg=true;
} }*/
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/ /*Set ROBOTS variable for barrier in .bzz from neighbours count*/
//no_of_robots=get_number_of_robots(); //no_of_robots=get_number_of_robots();
get_number_of_robots(); get_number_of_robots();
@ -142,7 +148,11 @@ namespace rosbzz_node{
n_c.getParam("stand_by", stand_by); n_c.getParam("stand_by", stand_by);
n_c.getParam("xbee_status_srv", xbeesrv_name); n_c.getParam("xbee_status_srv", xbeesrv_name);
if(n_c.getParam("xbee_plugged", xbeeplugged)); if(n_c.getParam("xbee_plugged", xbeeplugged));
else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");} 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); GetSubscriptionParameters(n_c);
// initialize topics to null? // initialize topics to null?
@ -205,13 +215,13 @@ namespace rosbzz_node{
/*publishers*/ /*publishers*/
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000); payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",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*/ /* Service Clients*/
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient); arm_client = n_c.serviceClient<mavros_msgs::CommandBool>("/"+robot_name+armclient);
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient); mode_client = n_c.serviceClient<mavros_msgs::SetMode>("/"+robot_name+modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name); mav_client = n_c.serviceClient<mavros_msgs::CommandLong>("/"+robot_name+fcclient_name);
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name); xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>("/"+robot_name+xbeesrv_name);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name); stream_client = n_c.serviceClient<mavros_msgs::StreamRate>("/"+robot_name+stream_client_name);
multi_msg=true; 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){ 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"){ 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"){ 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"){ 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"){ 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"){ 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; std::cout << "Subscribed to: " << it->first << endl;
@ -344,35 +354,35 @@ namespace rosbzz_node{
delete[] out; delete[] out;
delete[] payload_out_ptr; delete[] payload_out_ptr;
/*Check for updater message if present send*/ /*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; uint8_t* buff_send = 0;
uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());; uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());;
int tot=0; int tot=0;
mavros_msgs::Mavlink update_packets; mavros_msgs::Mavlink update_packets;
fprintf(stdout,"Transfering code \n"); fprintf(stdout,"Transfering code \n");
fprintf(stdout,"Sent Update packet Size: %u \n",updater_msgSize); 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); buff_send =(uint8_t*)malloc(sizeof(uint16_t)+updater_msgSize);
memset(buff_send, 0,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; *(uint16_t*)(buff_send + tot)=updater_msgSize;
//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize); //fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
/*Append updater msgs*/ // Append updater msgs
memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize); memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
tot += updater_msgSize; tot += updater_msgSize;
/*Destroy the updater out msg queue*/ // Destroy the updater out msg queue
destroy_out_msg_queue(); destroy_out_msg_queue();
uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t))); uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t)));
uint64_t* payload_64 = new uint64_t[total_size]; uint64_t* payload_64 = new uint64_t[total_size];
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t)); memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
free(buff_send); 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); update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT);
for(int i=0;i<total_size;i++){ for(int i=0;i<total_size;i++){
update_packets.payload64.push_back(payload_64[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; if (message_number < 0) message_number=0;
else message_number++; else message_number++;
update_packets.sysid=(uint8_t)robot_id; update_packets.sysid=(uint8_t)robot_id;
@ -380,7 +390,7 @@ namespace rosbzz_node{
payload_pub.publish(update_packets); payload_pub.publish(update_packets);
multi_msg=false; multi_msg=false;
delete[] payload_64; delete[] payload_64;
} }*/
} }
/*--------------------------------------------------------------------------------- /*---------------------------------------------------------------------------------
@ -807,7 +817,7 @@ namespace rosbzz_node{
void roscontroller::SetMode(std::string mode, int delay_miliseconds){ void roscontroller::SetMode(std::string mode, int delay_miliseconds){
// wait if necessary // wait if necessary
if (delay_miliseconds != 0){ if (delay_miliseconds != 0){
std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) ); ros::Duration(delay_miliseconds/1000).sleep();
} }
// set mode // set mode
mavros_msgs::SetMode set_mode_message; mavros_msgs::SetMode set_mode_message;