From 4deefe64d402e152e8e1a9e0a7bce8601bb35758 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Thu, 7 Sep 2017 18:03:53 -0400 Subject: [PATCH] Improved rosrate --- buzz_scripts/graphform.bzz | 2 +- src/roscontroller.cpp | 78 +++++++++++++++++++------------------- 2 files changed, 41 insertions(+), 39 deletions(-) diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index 339e6ce..722d186 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -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" diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 55ac7c1..55d5225 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -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<::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))