Improved rosrate
This commit is contained in:
parent
7268209d35
commit
4deefe64d4
|
@ -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"
|
||||||
|
|
||||||
|
|
|
@ -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,8 +245,8 @@ 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 << ","
|
||||||
|
@ -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))
|
||||||
|
|
Loading…
Reference in New Issue