Added searching_state.py

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>

path_planning/nodes/pole_search_demo.py

@@ -0,0 +1,52 @@
+#!/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
+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("pole_search_test")
+
+    target_depth = 3  # m
+
+    dive_machine = SequentialStateMachine('dive', [WaitingState(20), StabilizeState(),
+                                                   GoToDepthState(target_depth, tolerance=0.5,
+                                                                  velocity_tolerance=0.5),
+                                                   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)
+
+    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()

path_planning/src/path_planning/states/searching_state.py

@@ -0,0 +1,85 @@
+from path_planning.states.base_state import BaseState
+import math
+
+class SearchingState(BaseState):
+    """
+      Moves the submarine to the target position, assuming there are no obstacles in the way.
+     """
+
+    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 = 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,):
+        """
+        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
+        """
+
+        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
+            yield new_target_x, new_target_y
+            i += 1
+
+    def state_name(self):
+        return "searching_state"
+    def initialize(self, t, controls, sub_state, world_state, sensors):
+        if self.origin_x is None:
+            position = sub_state.get_submarine_state().position
+            self.origin_x = position.x
+        if self.origin_y is None:
+            position = sub_state.get_submarine_state().position
+            self.origin_y = position.y
+        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)
+
+        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):
+        position = sub_state.get_submarine_state().position
+        subpos_x = position.x
+        subpos_y = position.y
+        tolerance = 1
+        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
+
+    def finalize(self, t, controls, sub_state, world_state, sensors):
+        pass
+
+    def has_completed(self):
+        return self.completed
+
+    def exit_code(self):
+        return 0
+