From 72c86d478c778b6e688993832cf564384d5a93d4 Mon Sep 17 00:00:00 2001
From: adeya <adeyanju_charles@yahoo.ca>
Date: Sun, 24 Jan 2021 14:16:57 -0700
Subject: [PATCH 01/12] Added searching_state.py

---
 .../path_planning/states/searching_state.py   | 51 +++++++++++++++++++
 1 file changed, 51 insertions(+)
 create mode 100644 path_planning/src/path_planning/states/searching_state.py

diff --git a/path_planning/src/path_planning/states/searching_state.py b/path_planning/src/path_planning/states/searching_state.py
new file mode 100644
index 00000000..3848921e
--- /dev/null
+++ b/path_planning/src/path_planning/states/searching_state.py
@@ -0,0 +1,51 @@
+class SearchingState:
+    """
+    Each state should define how it interacts with the depth and orientation control systems, because those are
+    persisted across state changes.
+    """
+    def state_name(self):
+        return "searching_state"
+
+    def initialize(self, t, controls, sub_state, world_state, sensors):
+        """
+        Set up anything that needs initializing
+        Run EACH time the state is chosen as the next state
+        process(...) will be called with the next available data
+        Note the parameters passed into function may be the same as the ones first passed into process.
+        """
+        pass
+
+    def process(self, t, controls, sub_state, world_state, sensors):
+        """
+        This function will be called at a regular rate.
+        When the state has finished and wants to finalize, it should just ensure that has_completed returns True.
+        Do not manually call finalize, or else it will be called twice!
+        """
+        pass
+
+    def finalize(self, t, controls, sub_state, world_state, sensors):
+        """
+        Clean up anything necessary.
+        Note the parameters passed into function may be the same as the last ones passed into process.
+        Note this may be called before the state has completed. If this occurs,
+        the state should clean up any resources like normal; the exit_code in this case is not important.
+        """
+        pass
+
+    def has_completed(self):
+        """
+        When this function returns True, the state should exit.
+        Control may be passed to another state if this is part of a state machine.
+        The reason for exiting should be documented and returned in the exit_code function.
+        """
+        pass
+
+    def exit_code(self):
+        """
+        This function should only be called once has_completed returns True.
+        Indicates the reason that the state has exited.
+        The code 0 signifies that the state completed its objective and exited successfully.
+        The code -1 signifies that ros was shutdown.
+        Other codes should be numbered 1 and higher, and their explanations should be documented.
+        """
+        return 0

From 7a43d18ccbc4412ee97570c4903b98ce9f0b0976 Mon Sep 17 00:00:00 2001
From: cadeyanju <tadeyanj@uwaterloo.ca>
Date: Tue, 9 Feb 2021 16:45:58 -0700
Subject: [PATCH 02/12] Added searching_state.py

---
 .../path_planning/states/searching_state.py   | 90 +++++++++++--------
 1 file changed, 54 insertions(+), 36 deletions(-)

diff --git a/path_planning/src/path_planning/states/searching_state.py b/path_planning/src/path_planning/states/searching_state.py
index 3848921e..1d56cd7a 100644
--- a/path_planning/src/path_planning/states/searching_state.py
+++ b/path_planning/src/path_planning/states/searching_state.py
@@ -1,51 +1,69 @@
-class SearchingState:
-    """
-    Each state should define how it interacts with the depth and orientation control systems, because those are
-    persisted across state changes.
+from path_planning.states.base_state import BaseState
+import math
+
+class SearchingState(BaseState):
     """
-    def state_name(self):
-        return "searching_state"
+      Moves the submarine to the target position, assuming there are no obstacles in the way.
+     """
 
-    def initialize(self, t, controls, sub_state, world_state, sensors):
+    def __init__(self, target_x=None, target_y=None, target_z=None, target_yaw=None, target = ""):
         """
-        Set up anything that needs initializing
-        Run EACH time the state is chosen as the next state
-        process(...) will be called with the next available data
-        Note the parameters passed into function may be the same as the ones first passed into process.
+        This class assumes that the target roll and pitch are 0.
         """
-        pass
+        self.target = target
+        self.completed = False
+        self.checkpoints = self.spiral_calc(target_x, target_y)
+        self.target_x = target_x
+        self.target_y = target_y
+        self.target_z = target_z
+        self.target_yaw = target_yaw
 
-    def process(self, t, controls, sub_state, world_state, sensors):
+    def spiral_calc(self, target_x, target_y):
         """
-        This function will be called at a regular rate.
-        When the state has finished and wants to finalize, it should just ensure that has_completed returns True.
-        Do not manually call finalize, or else it will be called twice!
+        Calculates the x and y coordinates in a spiral
+        plot and updates an origin x and y coordinate by
+        accepting the target_x and target_y in its arguments
         """
-        pass
+        checkpoints = []
+        num_x_y_pts = 200
+        space_btw_lines = 5
+        velo_time_rate = 20
+        for i in range(num_x_y_pts):
+            t = i / velo_time_rate * pi
+            x = (1 + space_btw_lines * t) * cos(t)
+            y = (1 + space_btw_lines * t) * sin(t)
+            new_target_x = target_x + x
+            new_target_y = target_y + y
+            checkpoints.append((new_target_x, new_target_y))
+        return checkpoints
+
+
+    def state_name(self):
+        return "searching_state"
+    def initialize(self, t, controls, sub_state, world_state, sensors):
+        target_x, target_y = self.checkpoints[0]
+        controls.set_movement_target(target_x, target_y)
+
+    def process(self, t, controls, sub_state, world_state, sensors):
+        target_x, target_y = self.checkpoints[0]
+        position = sub_state.get_submarine_state().position
+        subpos_x = position.x
+        subpos_y = position.y
+        tolerance = 1
+        if abs(target_x - subpos_x) < tolerance and abs(target_y - subpos_y) < tolerance:
+            self.checkpoints.pop(0)
+            new_target_x, new_target_y = self.checkpoints[0]
+            controls.set_movement_target(new_target_x, new_target_y)
+
+        if self.target in world_state.get_world_state().keys():
+            self.completed = True
 
     def finalize(self, t, controls, sub_state, world_state, sensors):
-        """
-        Clean up anything necessary.
-        Note the parameters passed into function may be the same as the last ones passed into process.
-        Note this may be called before the state has completed. If this occurs,
-        the state should clean up any resources like normal; the exit_code in this case is not important.
-        """
         pass
 
     def has_completed(self):
-        """
-        When this function returns True, the state should exit.
-        Control may be passed to another state if this is part of a state machine.
-        The reason for exiting should be documented and returned in the exit_code function.
-        """
-        pass
+        return self.completed
 
     def exit_code(self):
-        """
-        This function should only be called once has_completed returns True.
-        Indicates the reason that the state has exited.
-        The code 0 signifies that the state completed its objective and exited successfully.
-        The code -1 signifies that ros was shutdown.
-        Other codes should be numbered 1 and higher, and their explanations should be documented.
-        """
         return 0
+

From e28632e1a4b7435dc31f03bed4d4cb82c7a0971f Mon Sep 17 00:00:00 2001
From: cadeyanju <tadeyanj@uwaterloo.ca>
Date: Tue, 9 Feb 2021 17:32:32 -0700
Subject: [PATCH 03/12] Added searching_state.py

---
 path_planning/nodes/pole_search_demo.py       | 36 +++++++++++++++++++
 .../path_planning/states/searching_state.py   | 16 ++++++---
 2 files changed, 47 insertions(+), 5 deletions(-)
 create mode 100644 path_planning/nodes/pole_search_demo.py

diff --git a/path_planning/nodes/pole_search_demo.py b/path_planning/nodes/pole_search_demo.py
new file mode 100644
index 00000000..a845da7c
--- /dev/null
+++ b/path_planning/nodes/pole_search_demo.py
@@ -0,0 +1,36 @@
+#!/usr/bin/env python3
+
+import rospy
+import rospkg
+from matplotlib import pyplot as plt
+from time import time
+
+from path_planning.states.waiting_state import WaitingState
+from path_planning.states.stabilize_state import StabilizeState
+from path_planning.states.go_to_depth import GoToDepthState
+from path_planning.states.exit_code_state import ExitCodeState
+from path_planning.states.data_logger import DataLogger
+from path_planning.state_machines.sequential_state_machine import SequentialStateMachine
+from path_planning.state_machines.parallel_state_machine import ParallelStateMachine
+from path_planning.state_executor import StateExecutor
+from path_planning.states.searching_state import SearchingState
+
+
+
+if __name__ == "__main__":
+    rospy.init_node("dive_test")
+
+    target_depth = 6  # m
+
+    dive_machine = SequentialStateMachine('dive', [WaitingState(20), StabilizeState(),
+                                                   GoToDepthState(target_depth, tolerance=0.05,
+                                                                  velocity_tolerance=0.01),
+                                                   WaitingState(10), SearchingState(target='blue_pole'), WaitingState(10), ExitCodeState(0)])
+    data_logger = DataLogger('dive-test')
+
+
+    dive_logging_machine = ParallelStateMachine('dive_logger', states=[dive_machine],
+                                                daemon_states=[data_logger])
+
+    executor = StateExecutor(dive_logging_machine, rate=rospy.Rate(5))
+    executor.run()
diff --git a/path_planning/src/path_planning/states/searching_state.py b/path_planning/src/path_planning/states/searching_state.py
index 1d56cd7a..0979d1f3 100644
--- a/path_planning/src/path_planning/states/searching_state.py
+++ b/path_planning/src/path_planning/states/searching_state.py
@@ -6,16 +6,16 @@ class SearchingState(BaseState):
       Moves the submarine to the target position, assuming there are no obstacles in the way.
      """
 
-    def __init__(self, target_x=None, target_y=None, target_z=None, target_yaw=None, target = ""):
+    def __init__(self, origin_x=None, origin_y=None, origin_z=None, target_yaw=None, target =""):
         """
         This class assumes that the target roll and pitch are 0.
         """
         self.target = target
         self.completed = False
-        self.checkpoints = self.spiral_calc(target_x, target_y)
-        self.target_x = target_x
-        self.target_y = target_y
-        self.target_z = target_z
+        self.checkpoints = None
+        self.origin_x = origin_x
+        self.origin_y = origin_y
+        self.origin_z = origin_z
         self.target_yaw = target_yaw
 
     def spiral_calc(self, target_x, target_y):
@@ -43,6 +43,12 @@ def state_name(self):
     def initialize(self, t, controls, sub_state, world_state, sensors):
         target_x, target_y = self.checkpoints[0]
         controls.set_movement_target(target_x, target_y)
+        if origin_x is None:
+            position = sub_state.get_submarine_state().position
+            self.origin_x = position.x
+        if origin_y is None:
+            position = sub_state.get_submarine_state().position
+            self.origin_y = position.y
 
     def process(self, t, controls, sub_state, world_state, sensors):
         target_x, target_y = self.checkpoints[0]

From 36062db11edde3a7641725ef7e7efdbd7c3bf8f4 Mon Sep 17 00:00:00 2001
From: Waterloo Aquadrone <waterlooaquadrone@gmail.com>
Date: Tue, 9 Feb 2021 20:02:11 -0500
Subject: [PATCH 04/12] created launch files

---
 .../launch/pole_search_demo.launch            | 23 +++++++++++++++++++
 path_planning/nodes/pole_search_demo.py       |  0
 2 files changed, 23 insertions(+)
 create mode 100755 aquadrone_sim_demos/launch/pole_search_demo.launch
 mode change 100644 => 100755 path_planning/nodes/pole_search_demo.py

diff --git a/aquadrone_sim_demos/launch/pole_search_demo.launch b/aquadrone_sim_demos/launch/pole_search_demo.launch
new file mode 100755
index 00000000..edb18024
--- /dev/null
+++ b/aquadrone_sim_demos/launch/pole_search_demo.launch
@@ -0,0 +1,23 @@
+<launch>
+
+    <include file="$(find aquadrone_sim_worlds)/launch/empty_world.launch">
+        <arg name="paused" value="false"/>
+    </include>
+
+    <include file="$(find aquadrone_description)/launch/upload_sub.launch">
+        <arg name="model_file" value="v28.urdf.xacro"/>
+    </include>
+
+    <include file="$(find aquadrone_description)/launch/spawn_sub.launch">
+        <arg name="model" value="aquadrone"/>
+    </include>
+
+    <include file="$(find thruster_control)/launch/sim_control.launch"/>
+    <include file="$(find stability)/launch/stability.launch" />
+
+    <node name="omniscient_state_est" pkg="state_estimation" type="omniscient_ekf_node.py"/>
+    <node name="omniscient_vision_node" pkg="vision" type="faker.py" />
+
+    <node name="pole_search" pkg="path_planning" type="pole_search_demo.py" output="screen"/>
+
+</launch>
diff --git a/path_planning/nodes/pole_search_demo.py b/path_planning/nodes/pole_search_demo.py
old mode 100644
new mode 100755

From 48540691be3dd29412304e151d52a174b4be1cc4 Mon Sep 17 00:00:00 2001
From: cadeyanju <tadeyanj@uwaterloo.ca>
Date: Tue, 9 Feb 2021 18:12:35 -0700
Subject: [PATCH 05/12] Added searching_state.py

---
 .../src/path_planning/states/searching_state.py  | 16 +++++++++++-----
 1 file changed, 11 insertions(+), 5 deletions(-)

diff --git a/path_planning/src/path_planning/states/searching_state.py b/path_planning/src/path_planning/states/searching_state.py
index 0979d1f3..d9d9fb0e 100644
--- a/path_planning/src/path_planning/states/searching_state.py
+++ b/path_planning/src/path_planning/states/searching_state.py
@@ -18,7 +18,8 @@ def __init__(self, origin_x=None, origin_y=None, origin_z=None, target_yaw=None,
         self.origin_z = origin_z
         self.target_yaw = target_yaw
 
-    def spiral_calc(self, target_x, target_y):
+    @staticmethod
+    def spiral_calc(origin_x, origin_y):
         """
         Calculates the x and y coordinates in a spiral
         plot and updates an origin x and y coordinate by
@@ -32,8 +33,8 @@ def spiral_calc(self, target_x, target_y):
             t = i / velo_time_rate * pi
             x = (1 + space_btw_lines * t) * cos(t)
             y = (1 + space_btw_lines * t) * sin(t)
-            new_target_x = target_x + x
-            new_target_y = target_y + y
+            new_target_x = origin_x + x
+            new_target_y = origin_y + y
             checkpoints.append((new_target_x, new_target_y))
         return checkpoints
 
@@ -41,14 +42,19 @@ def spiral_calc(self, target_x, target_y):
     def state_name(self):
         return "searching_state"
     def initialize(self, t, controls, sub_state, world_state, sensors):
-        target_x, target_y = self.checkpoints[0]
-        controls.set_movement_target(target_x, target_y)
         if origin_x is None:
             position = sub_state.get_submarine_state().position
             self.origin_x = position.x
         if origin_y is None:
             position = sub_state.get_submarine_state().position
             self.origin_y = position.y
+        if origin_z is None:
+            position = sub_state.get_submarine_state().position
+            self.origin_z = position.z
+        self.checkpoints = self.spiral_calc(self.origin_x, self.origin_y)
+
+        target_x, target_y = self.checkpoints[0]
+        controls.set_movement_target(target_x, target_y)
 
     def process(self, t, controls, sub_state, world_state, sensors):
         target_x, target_y = self.checkpoints[0]

From 16b8c622bc650beb247fbc4bca7e9f1db097071f Mon Sep 17 00:00:00 2001
From: cadeyanju <tadeyanj@uwaterloo.ca>
Date: Tue, 9 Feb 2021 18:26:19 -0700
Subject: [PATCH 06/12] Added searching_state.py

---
 path_planning/nodes/pole_search_demo.py                   | 4 ++--
 path_planning/src/path_planning/states/searching_state.py | 6 +++---
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/path_planning/nodes/pole_search_demo.py b/path_planning/nodes/pole_search_demo.py
index a845da7c..e2bc6d3b 100755
--- a/path_planning/nodes/pole_search_demo.py
+++ b/path_planning/nodes/pole_search_demo.py
@@ -23,8 +23,8 @@
     target_depth = 6  # m
 
     dive_machine = SequentialStateMachine('dive', [WaitingState(20), StabilizeState(),
-                                                   GoToDepthState(target_depth, tolerance=0.05,
-                                                                  velocity_tolerance=0.01),
+                                                   GoToDepthState(target_depth, tolerance=0.5,
+                                                                  velocity_tolerance=0.5),
                                                    WaitingState(10), SearchingState(target='blue_pole'), WaitingState(10), ExitCodeState(0)])
     data_logger = DataLogger('dive-test')
 
diff --git a/path_planning/src/path_planning/states/searching_state.py b/path_planning/src/path_planning/states/searching_state.py
index d9d9fb0e..9eb15377 100644
--- a/path_planning/src/path_planning/states/searching_state.py
+++ b/path_planning/src/path_planning/states/searching_state.py
@@ -42,13 +42,13 @@ def spiral_calc(origin_x, origin_y):
     def state_name(self):
         return "searching_state"
     def initialize(self, t, controls, sub_state, world_state, sensors):
-        if origin_x is None:
+        if self.origin_x is None:
             position = sub_state.get_submarine_state().position
             self.origin_x = position.x
-        if origin_y is None:
+        if self.origin_y is None:
             position = sub_state.get_submarine_state().position
             self.origin_y = position.y
-        if origin_z is None:
+        if self.origin_z is None:
             position = sub_state.get_submarine_state().position
             self.origin_z = position.z
         self.checkpoints = self.spiral_calc(self.origin_x, self.origin_y)

From 7d55df09baaf0adfa35d1026c76773f118cf0ffc Mon Sep 17 00:00:00 2001
From: cadeyanju <tadeyanj@uwaterloo.ca>
Date: Tue, 9 Feb 2021 18:32:40 -0700
Subject: [PATCH 07/12] Added searching_state.py

---
 path_planning/src/path_planning/states/searching_state.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/path_planning/src/path_planning/states/searching_state.py b/path_planning/src/path_planning/states/searching_state.py
index 9eb15377..d3a44aa9 100644
--- a/path_planning/src/path_planning/states/searching_state.py
+++ b/path_planning/src/path_planning/states/searching_state.py
@@ -30,7 +30,7 @@ def spiral_calc(origin_x, origin_y):
         space_btw_lines = 5
         velo_time_rate = 20
         for i in range(num_x_y_pts):
-            t = i / velo_time_rate * pi
+            t = i / velo_time_rate * math.pi
             x = (1 + space_btw_lines * t) * cos(t)
             y = (1 + space_btw_lines * t) * sin(t)
             new_target_x = origin_x + x

From d5080bb96054f1148b952f225c30dfb6bfb91323 Mon Sep 17 00:00:00 2001
From: cadeyanju <tadeyanj@uwaterloo.ca>
Date: Tue, 9 Feb 2021 18:36:43 -0700
Subject: [PATCH 08/12] Added searching_state.py

---
 path_planning/nodes/pole_search_demo.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/path_planning/nodes/pole_search_demo.py b/path_planning/nodes/pole_search_demo.py
index e2bc6d3b..1b5d122c 100755
--- a/path_planning/nodes/pole_search_demo.py
+++ b/path_planning/nodes/pole_search_demo.py
@@ -20,7 +20,7 @@
 if __name__ == "__main__":
     rospy.init_node("dive_test")
 
-    target_depth = 6  # m
+    target_depth = 3  # m
 
     dive_machine = SequentialStateMachine('dive', [WaitingState(20), StabilizeState(),
                                                    GoToDepthState(target_depth, tolerance=0.5,

From b192cdbcec4d049c3c374390c57d66266ee10f5f Mon Sep 17 00:00:00 2001
From: cadeyanju <tadeyanj@uwaterloo.ca>
Date: Tue, 9 Feb 2021 18:42:26 -0700
Subject: [PATCH 09/12] Added searching_state.py

---
 path_planning/src/path_planning/states/searching_state.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/path_planning/src/path_planning/states/searching_state.py b/path_planning/src/path_planning/states/searching_state.py
index d3a44aa9..51bbdf33 100644
--- a/path_planning/src/path_planning/states/searching_state.py
+++ b/path_planning/src/path_planning/states/searching_state.py
@@ -31,8 +31,8 @@ def spiral_calc(origin_x, origin_y):
         velo_time_rate = 20
         for i in range(num_x_y_pts):
             t = i / velo_time_rate * math.pi
-            x = (1 + space_btw_lines * t) * cos(t)
-            y = (1 + space_btw_lines * t) * sin(t)
+            x = (1 + space_btw_lines * t) * math.cos(t)
+            y = (1 + space_btw_lines * t) * math.sin(t)
             new_target_x = origin_x + x
             new_target_y = origin_y + y
             checkpoints.append((new_target_x, new_target_y))

From 98438ed39fcbc30aa18bb0fd19224ce80d629af7 Mon Sep 17 00:00:00 2001
From: cadeyanju <tadeyanj@uwaterloo.ca>
Date: Mon, 1 Mar 2021 20:16:46 -0700
Subject: [PATCH 10/12] Added searching_state.py

---
 path_planning/nodes/pole_search_demo.py       |  3 +-
 .../path_planning/states/searching_state.py   | 36 ++++++++++---------
 2 files changed, 22 insertions(+), 17 deletions(-)

diff --git a/path_planning/nodes/pole_search_demo.py b/path_planning/nodes/pole_search_demo.py
index 1b5d122c..a6621d19 100755
--- a/path_planning/nodes/pole_search_demo.py
+++ b/path_planning/nodes/pole_search_demo.py
@@ -14,6 +14,7 @@
 from path_planning.state_machines.parallel_state_machine import ParallelStateMachine
 from path_planning.state_executor import StateExecutor
 from path_planning.states.searching_state import SearchingState
+from path_planning.state_machines.timed_state_machine import TimedStateMachine
 
 
 
@@ -25,7 +26,7 @@
     dive_machine = SequentialStateMachine('dive', [WaitingState(20), StabilizeState(),
                                                    GoToDepthState(target_depth, tolerance=0.5,
                                                                   velocity_tolerance=0.5),
-                                                   WaitingState(10), SearchingState(target='blue_pole'), WaitingState(10), ExitCodeState(0)])
+                                                   WaitingState(10), TimedStateMachine(SearchingState(target='blue_pole'), 60), WaitingState(10), ExitCodeState(0)])
     data_logger = DataLogger('dive-test')
 
 
diff --git a/path_planning/src/path_planning/states/searching_state.py b/path_planning/src/path_planning/states/searching_state.py
index 51bbdf33..ebe2b60f 100644
--- a/path_planning/src/path_planning/states/searching_state.py
+++ b/path_planning/src/path_planning/states/searching_state.py
@@ -13,31 +13,32 @@ def __init__(self, origin_x=None, origin_y=None, origin_z=None, target_yaw=None,
         self.target = target
         self.completed = False
         self.checkpoints = None
+        self.target_x = None
+        self.target_y = None
         self.origin_x = origin_x
         self.origin_y = origin_y
         self.origin_z = origin_z
         self.target_yaw = target_yaw
 
     @staticmethod
-    def spiral_calc(origin_x, origin_y):
+    def spiral_calc(origin_x, origin_y,):
         """
         Calculates the x and y coordinates in a spiral
         plot and updates an origin x and y coordinate by
         accepting the target_x and target_y in its arguments
         """
-        checkpoints = []
-        num_x_y_pts = 200
-        space_btw_lines = 5
-        velo_time_rate = 20
-        for i in range(num_x_y_pts):
+
+        i = 0
+        while True:
+            space_btw_lines = 5
+            velo_time_rate = 20
             t = i / velo_time_rate * math.pi
             x = (1 + space_btw_lines * t) * math.cos(t)
             y = (1 + space_btw_lines * t) * math.sin(t)
             new_target_x = origin_x + x
             new_target_y = origin_y + y
-            checkpoints.append((new_target_x, new_target_y))
-        return checkpoints
-
+            yield new_target_x, new_target_y
+            i += 1
 
     def state_name(self):
         return "searching_state"
@@ -53,19 +54,22 @@ def initialize(self, t, controls, sub_state, world_state, sensors):
             self.origin_z = position.z
         self.checkpoints = self.spiral_calc(self.origin_x, self.origin_y)
 
-        target_x, target_y = self.checkpoints[0]
-        controls.set_movement_target(target_x, target_y)
+        self .target_x, self.target_y = next(self.checkpoints)
+        controls.set_movement_target(self.target_x, self.target_y)
+
+        """
+        target_x and target_y are points on the spiral
+        from the list of x and y checkpoints
+        """
 
     def process(self, t, controls, sub_state, world_state, sensors):
-        target_x, target_y = self.checkpoints[0]
         position = sub_state.get_submarine_state().position
         subpos_x = position.x
         subpos_y = position.y
         tolerance = 1
-        if abs(target_x - subpos_x) < tolerance and abs(target_y - subpos_y) < tolerance:
-            self.checkpoints.pop(0)
-            new_target_x, new_target_y = self.checkpoints[0]
-            controls.set_movement_target(new_target_x, new_target_y)
+        if abs(self.target_x - subpos_x) < tolerance and abs(self.target_y - subpos_y) < tolerance:
+            self.target_x, self.target_y = next(self.checkpoints)
+            controls.set_movement_target(self.target_x, self.target_y)
 
         if self.target in world_state.get_world_state().keys():
             self.completed = True

From 86132151d2bc316e11f389bd2180902d5065b78c Mon Sep 17 00:00:00 2001
From: cadeyanju <tadeyanj@uwaterloo.ca>
Date: Mon, 1 Mar 2021 20:27:07 -0700
Subject: [PATCH 11/12] Added searching_state.py

---
 path_planning/nodes/pole_search_demo.py | 19 +++++++++++++++++--
 1 file changed, 17 insertions(+), 2 deletions(-)

diff --git a/path_planning/nodes/pole_search_demo.py b/path_planning/nodes/pole_search_demo.py
index a6621d19..c776d0e3 100755
--- a/path_planning/nodes/pole_search_demo.py
+++ b/path_planning/nodes/pole_search_demo.py
@@ -15,11 +15,23 @@
 from path_planning.state_executor import StateExecutor
 from path_planning.states.searching_state import SearchingState
 from path_planning.state_machines.timed_state_machine import TimedStateMachine
+from path_planning.state_tree import Tree
+from matplotlib import pyplot as plt
+from time import time
 
+def plot_circling_data(data):
+    directory = rospkg.RosPack().get_path('path_planning')
+    plt.plot(data['t'], data['x'], label='x_pos')
+    plt.plot(data['t'], data['y'], label='y_pos')
+    plt.xlabel('Time(s)')
+    plt.ylabel('x and y positions(m)')
+    plt.title('Position vs Time')
+    plt.legend()
+    plt.savefig(f'{directory}/pole searching plot{time()}.png')
 
 
 if __name__ == "__main__":
-    rospy.init_node("dive_test")
+    rospy.init_node("pole_search_test")
 
     target_depth = 3  # m
 
@@ -27,11 +39,14 @@
                                                    GoToDepthState(target_depth, tolerance=0.5,
                                                                   velocity_tolerance=0.5),
                                                    WaitingState(10), TimedStateMachine(SearchingState(target='blue_pole'), 60), WaitingState(10), ExitCodeState(0)])
-    data_logger = DataLogger('dive-test')
+    data_logger = DataLogger('pole_search_test')
 
+    data_logger.add_data_post_processing_func(plot_circling_data)
 
     dive_logging_machine = ParallelStateMachine('dive_logger', states=[dive_machine],
                                                 daemon_states=[data_logger])
 
+    Tree.create_flowchart(dive_logging_machine, 'pole_search_travel_test')
+
     executor = StateExecutor(dive_logging_machine, rate=rospy.Rate(5))
     executor.run()

From 1a94c2942a68bef484be7c1facb0bc27afb4cfee Mon Sep 17 00:00:00 2001
From: cadeyanju <tadeyanj@uwaterloo.ca>
Date: Mon, 1 Mar 2021 22:25:36 -0700
Subject: [PATCH 12/12] Added searching_state.py

---
 path_planning/nodes/pole_search_demo.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/path_planning/nodes/pole_search_demo.py b/path_planning/nodes/pole_search_demo.py
index c776d0e3..3d48f3cd 100755
--- a/path_planning/nodes/pole_search_demo.py
+++ b/path_planning/nodes/pole_search_demo.py
@@ -38,7 +38,7 @@ def plot_circling_data(data):
     dive_machine = SequentialStateMachine('dive', [WaitingState(20), StabilizeState(),
                                                    GoToDepthState(target_depth, tolerance=0.5,
                                                                   velocity_tolerance=0.5),
-                                                   WaitingState(10), TimedStateMachine(SearchingState(target='blue_pole'), 60), WaitingState(10), ExitCodeState(0)])
+                                                   WaitingState(10), TimedStateMachine(SearchingState(target='blue_pole'), 10), WaitingState(10), ExitCodeState(0)])
     data_logger = DataLogger('pole_search_test')
 
     data_logger.add_data_post_processing_func(plot_circling_data)