increase delay on neigh garbage collector and switch gohome to manual

This commit is contained in:
dave 2018-11-12 17:44:03 -05:00
parent b3f9b7cf4e
commit 33e19e16ae
4 changed files with 20 additions and 17 deletions

View File

@ -54,7 +54,7 @@ function launch() {
function launch_switch() { function launch_switch() {
BVMSTATE = "LAUNCH_SWITCH" BVMSTATE = "LAUNCH_SWITCH"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF 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) { if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, AUTO_LAUNCH_STATE, 22) barrier_set(ROBOTS, AUTO_LAUNCH_STATE, AUTO_LAUNCH_STATE, 22)
barrier_ready(22) barrier_ready(22)
@ -74,11 +74,13 @@ gohomeT=100
function goinghome() { function goinghome() {
BVMSTATE = "GOHOME" BVMSTATE = "GOHOME"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF 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 storegoal(homegps.lat, homegps.lng, pose.position.altitude)
gohome() goto_gps(AUTO_LAUNCH_STATE)
gohomeT = gohomeT - 1 #if(gohomeT > 0) { # TODO: Make a real check if home is reached
} else # gohome()
BVMSTATE = AUTO_LAUNCH_STATE # gohomeT = gohomeT - 1
#} else
# BVMSTATE = AUTO_LAUNCH_STATE
} }
} }

View File

@ -33,7 +33,7 @@ function vec_from_gps(lat, lon, home_ref) {
d_lon = lon - pose.position.longitude d_lon = lon - pose.position.longitude
d_lat = lat - pose.position.latitude d_lat = lat - pose.position.latitude
if(home_ref == 1) { if(home_ref == 1) {
d_lon = lon - homegps.long d_lon = lon - homegps.lng
d_lat = lat - homegps.lat d_lat = lat - homegps.lat
} }
ned_x = d_lat/180*math.pi * 6371000.0; ned_x = d_lat/180*math.pi * 6371000.0;

View File

@ -118,7 +118,6 @@ private:
std::map<int, buzz_utility::Pos_struct> targets_found; std::map<int, buzz_utility::Pos_struct> targets_found;
std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map; std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
std::map<int, buzz_utility::neighbor_time> neighbours_time_map; std::map<int, buzz_utility::neighbor_time> neighbours_time_map;
int timer_step = 0;
int robot_id = 0; int robot_id = 0;
ros::Time logical_clock; ros::Time logical_clock;
ros::Time previous_step_time; ros::Time previous_step_time;
@ -222,7 +221,7 @@ private:
void prepare_msg_and_publish(); void prepare_msg_and_publish();
/*Refresh neighbours Position for every ten step*/ /*Refresh neighbours Position for every ten step*/
void maintain_pos(int tim_step); void clear_pos();
/*Puts neighbours position inside neigbours_pos_map*/ /*Puts neighbours position inside neigbours_pos_map*/
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr); void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr);

View File

@ -111,6 +111,7 @@ void roscontroller::RosControllerRun()
/rosbuzz_node main loop method /rosbuzz_node main loop method
/--------------------------------------------------*/ /--------------------------------------------------*/
{ {
int timer_step = 0;
// Set the Buzz bytecode // Set the Buzz bytecode
if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), robot_id)) 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 // Call Step from buzz script
buzz_utility::buzz_script_step(); 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 messages and publish them
prepare_msg_and_publish(); prepare_msg_and_publish();
// Call the flight controler service // Call the flight controler service
@ -185,9 +192,6 @@ void roscontroller::RosControllerRun()
buzzuav_closures::rc_call(NAV_LAND); buzzuav_closures::rc_call(NAV_LAND);
else else
fcu_timeout -= 1 / BUZZRATE; fcu_timeout -= 1 / BUZZRATE;
timer_step += 1;
// Force a refresh on neighbors array once in a while
maintain_pos(timer_step);
// DEBUG // DEBUG
// std::cout<< "HOME: " << home.latitude << ", " << home.longitude; // 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 /Refresh neighbours Position for every ten step
/---------------------------------------------*/ /---------------------------------------------*/
{ {
if (timer_step >= BUZZRATE)
{
neighbours_pos_map.clear(); neighbours_pos_map.clear();
raw_neighbours_pos_map.clear();
buzzuav_closures::clear_neighbours_pos(); buzzuav_closures::clear_neighbours_pos();
// raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but
// have to clear ! // have to clear !
timer_step = 0;
}
} }
void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr) void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr)