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)
uav_initstig()
# 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
UAVSTATE = "TURNEDOFF"

78
src/roscontroller.cpp
View File

@ -192,7 +192,9 @@ void roscontroller::RosControllerRun()
robot_id)) {
ROS_INFO("[%i] Bytecode file found and set", robot_id);
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
//////////////////////////////////////////////////////
@ -217,14 +219,14 @@ void roscontroller::RosControllerRun()
}
const uint8_t shrt_id= 0xFF;
float result;
if ( GetAPIRssi(shrt_id, result) )
log<<result<<",";
else
log<<"0,";
if ( GetRawPacketLoss(shrt_id, result) )
log<<result<<",";
else
log<<"0,";
//if ( GetAPIRssi(shrt_id, result) )
// log<<result<<",";
//else
// log<<"0,";
// if ( GetRawPacketLoss(shrt_id, result) )
// log<<result<<",";
//else
// log<<"0,";
if ( GetFilteredPacketLoss(shrt_id, result) )
log<<result<<",";
else
@ -233,7 +235,7 @@ void roscontroller::RosControllerRun()
neighbours_pos_publisher();
send_MPpayload();
/*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<<(int)buzz_utility::get_inmsg_size()<<",";
/*Step buzz script */
@ -243,13 +245,13 @@ void roscontroller::RosControllerRun()
/*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;
log << cur_pos.latitude << "," << cur_pos.longitude << ","
<< cur_pos.altitude << ",";
collect_data(log);
collect_data(log);
map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin();
log << "," << neighbours_pos_map.size();
@ -259,7 +261,7 @@ void roscontroller::RosControllerRun()
<< "," << (double)it->second.z;
}
log << std::endl;*/
}
//}
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
// no_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
SCRIPT IS NOT graphform.bzz*/
static buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1));
buzzvm_gload(VM);
buzzobj_t graph_state = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM);
//buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1));
// buzzvm_gload(VM);
// buzzobj_t graph_state = buzzvm_stack_at(VM, 1);
// buzzvm_pop(VM);
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_gload(VM);
buzzobj_t uav_state = buzzvm_stack_at(VM, 1);
@ -284,7 +286,7 @@ void roscontroller::RosControllerRun()
// buzz_utility::set_robot_var(no_of_robots);
/*Set no of robots for updates TODO only when not updating*/
// if(multi_msg)
updates_set_robots(no_of_robots);
//updates_set_robots(no_of_robots);
// ROS_INFO("ROBOTS: %i , acutal :
// %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1);
get_xbee_status();
@ -624,7 +626,7 @@ void roscontroller::prepare_msg_and_publish()
delete[] out;
delete[] payload_out_ptr;
/*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)
{
uint8_t *buff_send = 0;
@ -666,7 +668,7 @@ void roscontroller::prepare_msg_and_publish()
payload_pub.publish(update_packets);
multi_msg = false;
delete[] payload_64;
}
}*/
}
/*---------------------------------------------------------------------------------
/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;
float result_float;
const uint8_t all_ids = 0xFF;
if(GetDequeFull(result_bool))
{
buzzuav_closures::set_deque_full(result_bool);
}
if(GetRssi(result_float))
{
buzzuav_closures::set_rssi(result_float);
}
if(GetRawPacketLoss(all_ids, result_float))
{
buzzuav_closures::set_raw_packet_loss(result_float);
}
if(GetFilteredPacketLoss(all_ids, result_float))
{
buzzuav_closures::set_filtered_packet_loss(result_float);
}
//if(GetDequeFull(result_bool))
//{
// buzzuav_closures::set_deque_full(result_bool);
//}
//if(GetRssi(result_float))
//{
// buzzuav_closures::set_rssi(result_float);
//}
//if(GetRawPacketLoss(all_ids, result_float))
//{
// buzzuav_closures::set_raw_packet_loss(result_float);
//}
//if(GetFilteredPacketLoss(all_ids, result_float))
//{
// buzzuav_closures::set_filtered_packet_loss(result_float);
//}
// This part needs testing since it can overload the xbee module
/*
* if(GetAPIRssi(all_ids, result_float))