From 4deefe64d402e152e8e1a9e0a7bce8601bb35758 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Thu, 7 Sep 2017 18:03:53 -0400 Subject: [PATCH 1/9] 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)) From f03b76c241437b6f458a16d6ce0d453d2455b996 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Fri, 8 Sep 2017 12:23:23 -0400 Subject: [PATCH 2/9] merge confilct manual resolution --- buzz_scripts/graphform.bzz | 4 ---- 1 file changed, 4 deletions(-) diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index 1b397e9..ef03438 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -797,11 +797,7 @@ function init() { v_tag = stigmergy.create(m_lockstig) uav_initstig() # go to diff. height since no collision avoidance implemented yet -<<<<<<< HEAD - TARGET_ALTITUDE = 4.5 #2.5 + id * 1.5 -======= TARGET_ALTITUDE = 7.5 #2.5 + id * 1.5 ->>>>>>> d587cdba4387b65bcc583671d26ce82fa4e25720 statef=turnedoff UAVSTATE = "TURNEDOFF" } From 79d746d9ed6c5e95ad1c8bab5ebea540bf8673c2 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Fri, 8 Sep 2017 19:29:45 +0000 Subject: [PATCH 3/9] change TX launch --- misc/cmdlinectr.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index 2de3326..31f2061 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -31,6 +31,6 @@ function stoprobot { } function updaterobot { # rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch - rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch -# rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch +# rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch + rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch } From dafe876e15228af992134283254fb5afceb14cc6 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Sun, 10 Sep 2017 22:22:16 -0400 Subject: [PATCH 4/9] tklaunch filechange --- misc/cmdlinectr.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index 31f2061..2de3326 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -31,6 +31,6 @@ function stoprobot { } function updaterobot { # rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch -# rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch - rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch + rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch +# rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch } From ad883f8a798e71a2591937b4386f6a8854a7d5ed Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Sun, 10 Sep 2017 22:47:20 -0400 Subject: [PATCH 5/9] addition of 5 historic logs to preserve --- src/roscontroller.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index bed9e10..49ceb38 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -52,9 +52,11 @@ roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv) path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/"; std::string folder_check="mkdir -p "+path; system(folder_check.c_str()); - rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+".log").c_str(), - (path +"logger_"+ std::to_string((uint8_t)robot_id)+"_old.log").c_str()); - path += "logger_"+std::to_string(robot_id)+".log"; + for(int i=5;i>0;i--){ + rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i-1)+".log").c_str(), + (path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i)+".log").c_str()); + } + path += "logger_"+std::to_string(robot_id)+"_0.log"; log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out); } From ded07aa5e881343223359fe60b6cd74b4249df1b Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Sun, 10 Sep 2017 23:30:57 -0400 Subject: [PATCH 6/9] addition of highlevel packet drop --- src/roscontroller.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 49ceb38..2e4a284 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -195,6 +195,8 @@ void roscontroller::RosControllerRun() /* Set the Buzz bytecode */ if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), robot_id)) { + int packet_loss_tmp,time_step=0; + double cur_packet_loss=0; 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()); @@ -233,7 +235,19 @@ 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()); + if(time_step==BUZZRATE){ + time_step=0; + cur_packet_loss= 1 -( (double)packet_loss_tmp/(BUZZRATE*( (int)no_of_robots-1 )) ); + packet_loss_tmp=0; + if(cur_packet_loss<0) cur_packet_loss=0; + else if (cur_packet_loss>1) cur_packet_loss=1; + } + else{ + packet_loss_tmp+=(int)buzz_utility::get_inmsg_size(); + time_step++; + } + ROS_WARN("CURRENT PACKET DROP : %f ",cur_packet_loss); /*Log In Msg queue size*/ log<<(int)buzz_utility::get_inmsg_size()<<","; /*Step buzz script */ From 80fd376a8c1243d103aab4133ef2a6309dc71594 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 11 Sep 2017 18:01:56 -0400 Subject: [PATCH 7/9] field modifications 09/11 --- buzz_scripts/graphformGPS.bzz | 10 +++++----- buzz_scripts/include/barrier.bzz | 4 ++-- buzz_scripts/include/uavstates.bzz | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz index 3588ed8..a85fcad 100644 --- a/buzz_scripts/graphformGPS.bzz +++ b/buzz_scripts/graphformGPS.bzz @@ -5,7 +5,7 @@ include "string.bzz" include "vec2.bzz" include "update.bzz" -include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header. +#include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header. include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header. include "uavstates.bzz" # require an 'action' function to be defined here. @@ -14,7 +14,7 @@ include "graphs/shapes_Y.bzz" ROBOT_RADIUS = 50 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER -ROOT_ID = 2 +ROOT_ID = 13 # max velocity in cm/step ROBOT_MAXVEL = 150.0 @@ -704,9 +704,9 @@ function init() { # uav_initswarm() #v_tag = stigmergy.create(m_lockstig) - uav_initstig() + #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" } @@ -720,7 +720,7 @@ function step() { # get the swarm commands uav_neicmd() # update the vstig (status/net/batt) - uav_updatestig() + #uav_updatestig() #update the graph UpdateNodeInfo() diff --git a/buzz_scripts/include/barrier.bzz b/buzz_scripts/include/barrier.bzz index 3a029b0..9b1658e 100644 --- a/buzz_scripts/include/barrier.bzz +++ b/buzz_scripts/include/barrier.bzz @@ -7,7 +7,7 @@ # # Constants # -BARRIER_TIMEOUT = 200 # in steps +BARRIER_TIMEOUT = 1200 # in steps BARRIER_VSTIG = 80 timeW = 0 barrier = nil @@ -82,4 +82,4 @@ function getlowest(){ u=u-1 } log("--> LOWEST ID:",Lid) -} \ No newline at end of file +} diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 48a28a7..fa8f6d3 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -152,7 +152,7 @@ function uav_rccmd() { function uav_neicmd() { neighbors.listen("cmd", function(vid, value, rid) { - print("Got (", vid, ",", value, ") from robot #", rid, "(", UAVSTATE, ")") + print("Got (", vid, ",", value, ") #", rid, "(", UAVSTATE, ")") if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") { statef=takeoff UAVSTATE = "TAKEOFF" From 0eb07f973d53d0cf7001f09b709c0618fa19d56f Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 11 Sep 2017 23:02:04 -0400 Subject: [PATCH 8/9] spinner sync --- src/roscontroller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 2e4a284..c578ea8 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -227,10 +227,10 @@ void roscontroller::RosControllerRun() const uint8_t shrt_id= 0xFF; float result; - if ( GetFilteredPacketLoss(shrt_id, result) ) - log< Date: Fri, 15 Sep 2017 11:15:03 -0400 Subject: [PATCH 9/9] field improvement to graphformGPS --- buzz_scripts/graphformGPS.bzz | 55 +++++++++++++++++++++++++---------- 1 file changed, 39 insertions(+), 16 deletions(-) diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz index a85fcad..8a30502 100644 --- a/buzz_scripts/graphformGPS.bzz +++ b/buzz_scripts/graphformGPS.bzz @@ -9,7 +9,7 @@ include "update.bzz" include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header. include "uavstates.bzz" # require an 'action' function to be defined here. -include "graphs/shapes_Y.bzz" +include "graphs/shapes_L.bzz" ROBOT_RADIUS = 50 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS @@ -93,7 +93,7 @@ step_cunt=0 m_lockstig = 1 # Lennard-Jones parameters, may need change -EPSILON = 1800 #13.5 the LJ parameter for other robots +EPSILON = 4000 #13.5 the LJ parameter for other robots # Lennard-Jones interaction magnitude @@ -311,6 +311,8 @@ neighbors.listen("m", m_MessageRange[m_neighbourCount]=m_receivedMessage.Range m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing m_messageID[m_neighbourCount]=rid +log(rid, " is in ", m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount]) + m_neighbourCount=m_neighbourCount+1 }) } @@ -496,19 +498,24 @@ function DoAsking(){ log("get response") psResponse=i }} + if(m_MessageState[i]=="STATE_JOINING" and m_MessageLabel[i]==m_nLabel){ + TransitionToFree() + return + } i=i+1 } #analyse response if(psResponse==-1){ #no response, wait + m_unWaitCount=m_unWaitCount-1 m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.ReqLabel=m_nLabel m_selfMessage.ReqID=m_unRequestId - if(m_unWaitCount==0){ - TransitionToFree() - return - } + #if(m_unWaitCount==0){ + #TransitionToFree() + #return + #} } else{ log("respond id=",m_MessageReqID[psResponse]) @@ -528,6 +535,8 @@ function DoAsking(){ } } } + m_selfMessage.Label=m_nLabel + m_navigation.x=0.0 m_navigation.y=0.0 uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) @@ -578,6 +587,9 @@ function set_rc_goto() { # #Do joined # +repeat_assign=0 +assign_label=-1 +assign_id=-1 function DoJoined(){ m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel @@ -609,6 +621,12 @@ function DoJoined(){ var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias} } } + + if(m_MessageState[i]=="STATE_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){ + repeat_assign=0 + } + + #if it is the pred if((m_MessageState[i]=="STATE_JOINED" or m_MessageState[i]=="STATE_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){ seenPred=1 @@ -627,16 +645,21 @@ function DoJoined(){ ReqIndex=i i=i+1 } - #get the best index, whose ReqLabel and Reqid are - ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]] - var ReqID=m_MessageReqID[mapRequests[ReqIndex]] - m_selfMessage.ReqLabel=ReqLabel - m_selfMessage.ReqID=ReqID + if(repeat_assign==0){ + #get the best index, whose ReqLabel and Reqid are + ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]] + var ReqID=m_MessageReqID[mapRequests[ReqIndex]] + assign_label=ReqLabel + assign_id=ReqID + repeat_assign=1 + } + m_selfMessage.ReqLabel=assign_label + m_selfMessage.ReqID=assign_id m_selfMessage.Response=r2i("REQ_GRANTED") - m_vecNodes[ReqLabel].State="ASSIGNING" - log("Label=",ReqLabel) - log("ID=",ReqID) - m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod + #m_vecNodes[ReqLabel].State="ASSIGNING" + log("Label=",assign_label) + log("ID=",assign_id) + m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod } #lost pred, wait for some time and transit to free @@ -706,7 +729,7 @@ function init() { #v_tag = stigmergy.create(m_lockstig) #uav_initstig() # go to diff. height since no collision avoidance implemented yet - TARGET_ALTITUDE = 4.5 #2.5 + id * 1.5 + TARGET_ALTITUDE = 6.0 #2.5 + id * 1.5 statef=turnedoff UAVSTATE = "TURNEDOFF" }