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

Improved rosrate

This commit is contained in:
David St-Onge 2017-09-07 18:03:53 -04:00
parent 7268209d35
commit 4deefe64d4
2 changed files with 41 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
buzz_scripts/graphform.bzz
View File

@ -795,7 +795,7 @@ function init() {
v_tag = stigmergy.create(m_lockstig) v_tag = stigmergy.create(m_lockstig)
uav_initstig() uav_initstig()
# go to diff. height since no collision avoidance implemented yet # go to diff. height since no collision avoidance implemented yet
TARGET_ALTITUDE = 2.5 + id * 1.5 TARGET_ALTITUDE = 4.5 #2.5 + id * 1.5
statef=turnedoff statef=turnedoff
UAVSTATE = "TURNEDOFF" UAVSTATE = "TURNEDOFF"

78
src/roscontroller.cpp
View File

@ -192,7 +192,9 @@ void roscontroller::RosControllerRun()
robot_id)) { robot_id)) {
ROS_INFO("[%i] Bytecode file found and set", robot_id); ROS_INFO("[%i] Bytecode file found and set", robot_id);
std::string standby_bo = Compile_bzz(stand_by) + ".bo"; std::string standby_bo = Compile_bzz(stand_by) + ".bo";
init_update_monitor(bcfname.c_str(), standby_bo.c_str()); //init_update_monitor(bcfname.c_str(), standby_bo.c_str());
/*loop rate of ros*/
ros::Rate loop_rate(15);
/////////////////////////////////////////////////////// ///////////////////////////////////////////////////////
// MAIN LOOP // MAIN LOOP
////////////////////////////////////////////////////// //////////////////////////////////////////////////////
@ -217,14 +219,14 @@ void roscontroller::RosControllerRun()
} }
const uint8_t shrt_id= 0xFF; const uint8_t shrt_id= 0xFF;
float result; float result;
if ( GetAPIRssi(shrt_id, result) ) //if ( GetAPIRssi(shrt_id, result) )
log<<result<<","; // log<<result<<",";
else //else
log<<"0,"; // log<<"0,";
if ( GetRawPacketLoss(shrt_id, result) ) // if ( GetRawPacketLoss(shrt_id, result) )
log<<result<<","; // log<<result<<",";
else //else
log<<"0,"; // log<<"0,";
if ( GetFilteredPacketLoss(shrt_id, result) ) if ( GetFilteredPacketLoss(shrt_id, result) )
log<<result<<","; log<<result<<",";
else else
@ -233,7 +235,7 @@ void roscontroller::RosControllerRun()
neighbours_pos_publisher(); neighbours_pos_publisher();
send_MPpayload(); send_MPpayload();
/*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());
/*Log In Msg queue size*/ /*Log In Msg queue size*/
log<<(int)buzz_utility::get_inmsg_size()<<","; log<<(int)buzz_utility::get_inmsg_size()<<",";
/*Step buzz script */ /*Step buzz script */
@ -243,13 +245,13 @@ void roscontroller::RosControllerRun()
/*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;
log << cur_pos.latitude << "," << cur_pos.longitude << "," log << cur_pos.latitude << "," << cur_pos.longitude << ","
<< cur_pos.altitude << ","; << cur_pos.altitude << ",";
collect_data(log); collect_data(log);
map<int, buzz_utility::Pos_struct>::iterator it = map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin(); neighbours_pos_map.begin();
log << "," << neighbours_pos_map.size(); log << "," << neighbours_pos_map.size();
@ -259,7 +261,7 @@ void roscontroller::RosControllerRun()
<< "," << (double)it->second.z; << "," << (double)it->second.z;
} }
log << std::endl;*/ log << std::endl;*/
} //}
/*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();
@ -267,12 +269,12 @@ void roscontroller::RosControllerRun()
/*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF /*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF
SCRIPT IS NOT graphform.bzz*/ SCRIPT IS NOT graphform.bzz*/
static buzzvm_t VM = buzz_utility::get_vm(); static buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1)); //buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1));
buzzvm_gload(VM); // buzzvm_gload(VM);
buzzobj_t graph_state = buzzvm_stack_at(VM, 1); // buzzobj_t graph_state = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM); // buzzvm_pop(VM);
std::stringstream state_buff; std::stringstream state_buff;
state_buff<< graph_state->s.value.str<<","; // state_buff<< graph_state->s.value.str<<",";
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1));
buzzvm_gload(VM); buzzvm_gload(VM);
buzzobj_t uav_state = buzzvm_stack_at(VM, 1); buzzobj_t uav_state = buzzvm_stack_at(VM, 1);
@ -284,7 +286,7 @@ void roscontroller::RosControllerRun()
// buzz_utility::set_robot_var(no_of_robots); // buzz_utility::set_robot_var(no_of_robots);
/*Set no of robots for updates TODO only when not updating*/ /*Set no of robots for updates TODO only when not updating*/
// if(multi_msg) // if(multi_msg)
updates_set_robots(no_of_robots); //updates_set_robots(no_of_robots);
// ROS_INFO("ROBOTS: %i , acutal : // ROS_INFO("ROBOTS: %i , acutal :
// %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1); // %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1);
get_xbee_status(); get_xbee_status();
@ -624,7 +626,7 @@ void roscontroller::prepare_msg_and_publish()
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 && /* if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 &&
multi_msg) multi_msg)
{ {
uint8_t *buff_send = 0; uint8_t *buff_send = 0;
@ -666,7 +668,7 @@ void roscontroller::prepare_msg_and_publish()
payload_pub.publish(update_packets); payload_pub.publish(update_packets);
multi_msg = false; multi_msg = false;
delete[] payload_64; delete[] payload_64;
} }*/
} }
/*--------------------------------------------------------------------------------- /*---------------------------------------------------------------------------------
/Flight controller service call every step if there is a command set from bzz /Flight controller service call every step if there is a command set from bzz
@ -1239,22 +1241,22 @@ void roscontroller::get_xbee_status()
bool result_bool; bool result_bool;
float result_float; float result_float;
const uint8_t all_ids = 0xFF; const uint8_t all_ids = 0xFF;
if(GetDequeFull(result_bool)) //if(GetDequeFull(result_bool))
{ //{
buzzuav_closures::set_deque_full(result_bool); // buzzuav_closures::set_deque_full(result_bool);
} //}
if(GetRssi(result_float)) //if(GetRssi(result_float))
{ //{
buzzuav_closures::set_rssi(result_float); // buzzuav_closures::set_rssi(result_float);
} //}
if(GetRawPacketLoss(all_ids, result_float)) //if(GetRawPacketLoss(all_ids, result_float))
{ //{
buzzuav_closures::set_raw_packet_loss(result_float); // buzzuav_closures::set_raw_packet_loss(result_float);
} //}
if(GetFilteredPacketLoss(all_ids, result_float)) //if(GetFilteredPacketLoss(all_ids, result_float))
{ //{
buzzuav_closures::set_filtered_packet_loss(result_float); // buzzuav_closures::set_filtered_packet_loss(result_float);
} //}
// This part needs testing since it can overload the xbee module // This part needs testing since it can overload the xbee module
/* /*
* if(GetAPIRssi(all_ids, result_float)) * if(GetAPIRssi(all_ids, result_float))