From 33e19e16aedb33f792088967064e5c5da8de13ac Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 12 Nov 2018 17:44:03 -0500 Subject: [PATCH] increase delay on neigh garbage collector and switch gohome to manual --- buzz_scripts/include/act/states.bzz | 14 ++++++++------ buzz_scripts/include/utils/conversions.bzz | 2 +- include/roscontroller.h | 3 +-- src/roscontroller.cpp | 18 ++++++++++-------- 4 files changed, 20 insertions(+), 17 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index c766315..a0b1903 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -54,7 +54,7 @@ function launch() { function launch_switch() { BVMSTATE = "LAUNCH_SWITCH" if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF - homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} + homegps = {.lat=pose.position.latitude, .lng=pose.position.longitude} if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { barrier_set(ROBOTS, AUTO_LAUNCH_STATE, AUTO_LAUNCH_STATE, 22) barrier_ready(22) @@ -74,11 +74,13 @@ gohomeT=100 function goinghome() { BVMSTATE = "GOHOME" if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF - if(gohomeT > 0) { # TODO: Make a real check if home is reached - gohome() - gohomeT = gohomeT - 1 - } else - BVMSTATE = AUTO_LAUNCH_STATE + storegoal(homegps.lat, homegps.lng, pose.position.altitude) + goto_gps(AUTO_LAUNCH_STATE) + #if(gohomeT > 0) { # TODO: Make a real check if home is reached + # gohome() + # gohomeT = gohomeT - 1 + #} else + # BVMSTATE = AUTO_LAUNCH_STATE } } diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz index 28a88d3..e060e57 100644 --- a/buzz_scripts/include/utils/conversions.bzz +++ b/buzz_scripts/include/utils/conversions.bzz @@ -33,7 +33,7 @@ function vec_from_gps(lat, lon, home_ref) { d_lon = lon - pose.position.longitude d_lat = lat - pose.position.latitude if(home_ref == 1) { - d_lon = lon - homegps.long + d_lon = lon - homegps.lng d_lat = lat - homegps.lat } ned_x = d_lat/180*math.pi * 6371000.0; diff --git a/include/roscontroller.h b/include/roscontroller.h index 599f116..0f2bc2f 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -118,7 +118,6 @@ private: std::map targets_found; std::map raw_neighbours_pos_map; std::map neighbours_time_map; - int timer_step = 0; int robot_id = 0; ros::Time logical_clock; ros::Time previous_step_time; @@ -222,7 +221,7 @@ private: void prepare_msg_and_publish(); /*Refresh neighbours Position for every ten step*/ - void maintain_pos(int tim_step); + void clear_pos(); /*Puts neighbours position inside neigbours_pos_map*/ void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index b198752..cd8714c 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -111,6 +111,7 @@ void roscontroller::RosControllerRun() /rosbuzz_node main loop method /--------------------------------------------------*/ { + int timer_step = 0; // Set the Buzz bytecode if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), robot_id)) { @@ -161,6 +162,12 @@ void roscontroller::RosControllerRun() // Call Step from buzz script buzz_utility::buzz_script_step(); + // Force a refresh on neighbors array once in a while + if (timer_step >= 20*BUZZRATE){ + clear_pos(); + timer_step = 0; + } else + timer_step++; // Prepare messages and publish them prepare_msg_and_publish(); // Call the flight controler service @@ -185,9 +192,6 @@ void roscontroller::RosControllerRun() buzzuav_closures::rc_call(NAV_LAND); else fcu_timeout -= 1 / BUZZRATE; - timer_step += 1; - // Force a refresh on neighbors array once in a while - maintain_pos(timer_step); // DEBUG // std::cout<< "HOME: " << home.latitude << ", " << home.longitude; } @@ -941,19 +945,17 @@ script } } -void roscontroller::maintain_pos(int tim_step) +void roscontroller::clear_pos() /* /Refresh neighbours Position for every ten step /---------------------------------------------*/ { - if (timer_step >= BUZZRATE) - { + neighbours_pos_map.clear(); + raw_neighbours_pos_map.clear(); buzzuav_closures::clear_neighbours_pos(); // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but // have to clear ! - timer_step = 0; - } } void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr)