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 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/path_planning/nodes/pole_search_demo.py b/path_planning/nodes/pole_search_demo.py new file mode 100755 index 00000000..3d48f3cd --- /dev/null +++ b/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() 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..ebe2b60f --- /dev/null +++ b/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 +