From 66cb2df693c475b3479ffad223dc79ec5c56b62a Mon Sep 17 00:00:00 2001
From: vivek-shankar <vivek-shankar.varadharajan@polymtl.ca>
Date: Tue, 12 Jun 2018 14:30:14 -0400
Subject: [PATCH] Fixed neighbours pos clearing.

---
 include/buzzuav_closures.h | 4 ++++
 src/buzzuav_closures.cpp   | 5 ++++-
 src/roscontroller.cpp      | 1 +
 3 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h
index 6e31ec6..de887ea 100644
--- a/include/buzzuav_closures.h
+++ b/include/buzzuav_closures.h
@@ -101,6 +101,10 @@ void neighbour_pos_callback(int id, float range, float bearing, float elevation)
  * update neighbors from in msgs
  */
 void update_neighbors(buzzvm_t vm);
+/*
+ *Clear neighbours struct
+ */
+void clear_neighbours_pos();
 /*
  * closure to add a neighbor status
  */
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index abff74f..ce9fda9 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -651,7 +651,10 @@ void neighbour_pos_callback(int id, float range, float bearing, float elevation)
     neighbors_map.erase(it);
   neighbors_map.insert(make_pair(id, pos_arr));
 }
-
+// Clear neighbours pos 
+void clear_neighbours_pos(){
+  neighbors_map.clear();
+}
 //  update at each step the VM table
 void update_neighbors(buzzvm_t vm)
 {
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 010107e..e460bd7 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -732,6 +732,7 @@ void roscontroller::maintain_pos(int tim_step)
   if (timer_step >= BUZZRATE)
   {
     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;