diff --git a/ws_nodes_py/CVs/CVCamera.py b/ws_nodes_py/CVs/CVCamera.py index 5faaace41f7aaec7d414744448e7e4363b5124f0..602d6e6936b28b66d5d95eddc913e7146f6b114b 100644 --- a/ws_nodes_py/CVs/CVCamera.py +++ b/ws_nodes_py/CVs/CVCamera.py @@ -80,9 +80,9 @@ class CVCamera(GetParameterNode, ObjectsDetection): # TODO расчёт дистанции x, y, w, h = cv2.boundingRect(contour) - dist = 1 if w <= 40 else 0.1 + dist = 1 if cv2.moments(contour)["m00"] >= 500 else 0.1 - course = (self.get_course(contour) + 10) % 360 + course = (self.get_course(contour) - 10) % 360 posters.append((dist, course)) diff --git a/ws_nodes_py/CVs/videoS.py b/ws_nodes_py/CVs/videoS.py index c9478e39b1f721e253ef2a35c93a0e60d286baa3..807290b3eff6e3daaa58654fd18ee6e00cb24c83 100644 --- a/ws_nodes_py/CVs/videoS.py +++ b/ws_nodes_py/CVs/videoS.py @@ -6,7 +6,7 @@ from ws_nodes_py.CVs.CVCamera import CVCamera class RosOpencvToBinary(CVCamera): def __init__(self): - super().__init__('camcvS', 90) + super().__init__('camcvS', -90) # см. ~/Desktop/python/calc.py self.k1 = self.get_declare_parameter('k1', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) diff --git a/ws_nodes_py/StateMachine/MotionStates.py b/ws_nodes_py/StateMachine/MotionStates.py index d04da478d4a4867b149a9b18a9ce3e981abe10ef..b3c7cb18754f82fec5c9b7944a6f89c932492f9c 100644 --- a/ws_nodes_py/StateMachine/MotionStates.py +++ b/ws_nodes_py/StateMachine/MotionStates.py @@ -130,6 +130,31 @@ class MoveByVariableTwist(MoveByTwist): self.variants = kwargs def execute(self, userdata): - self.goal = self.variants[self.cntr.get_settings()[self.param_name]] - self.cntr.get_logger().info(f'Move by twist {round(self.goal[0] * 100), round(self.goal[1] * 100)} %, use {self.cntr.get_settings()[self.param_name]} parameter') - return super().execute(userdata) \ No newline at end of file + if self.param_name in self.cntr.get_settings(): + self.goal = self.variants[self.cntr.get_settings()[self.param_name]] + else: + self.goal = self.variants[self.cntr.get_settings()[self.variants.keys()[0]]] + # self.cntr.get_logger().info(f'Move by twist {round(self.goal[0] * 100), round(self.goal[1] * 100)} %, use {self.cntr.get_settings()[self.param_name]} parameter') + return super().execute(userdata) + + +class MoveByHeading(State): + def __init__(self, c, tar_heading:int=None, thrust:float=None): + State.__init__(self, outcomes=['complite'], input_keys=['sm_counter'], + output_keys=['sm_goal', 'sm_pos', 'sm_error_dist', 'sm_error_angle'],) + + self.cntr = c + + if tar_heading is None: + raise Exception("TARGET ANGLE IS NOT DESCRIBED") + target_heading = tar_heading + thrust = thrust if thrust is float or thrust else 0.1 + self.goal = [thrust, 0, target_heading] + + def execute(self, userdata): + self.cntr.next_step("MOVE HEADING", self.goal) + self.cntr.get_logger().info(f'Move by heading {round(self.goal[0] * 100)} % {round(self.goal[2])} deg') + while not self.cntr.shutdown(): + self.cntr.update() + rclpy.spin_once(self.cntr) + return 'complite' \ No newline at end of file diff --git a/ws_nodes_py/StateMachine/SpecialStates.py b/ws_nodes_py/StateMachine/SpecialStates.py index eff92aa835e4481e0d7afd2a549b9c5c7f28fc85..1be79251b590889d5a56e04889e87ef80e009452 100644 --- a/ws_nodes_py/StateMachine/SpecialStates.py +++ b/ws_nodes_py/StateMachine/SpecialStates.py @@ -68,7 +68,7 @@ class DropZabralo(State): self.is_close = is_close if is_close is not None else True def execute(self, userdata): - self.cntr.get_logger().info(f'Drop zabralo') + self.cntr.get_logger().info(f'{"Drop" if self.is_close else "Up"} zabralo') self.cntr.pub_zabralo(self.is_close) return 'complite' diff --git a/ws_nodes_py/__pycache__/regulator.cpython-310.pyc b/ws_nodes_py/__pycache__/regulator.cpython-310.pyc index 4d4e2a9273d23e4012336257d0040b8c80cafa86..62a35ad09c95d6e8f246e79c573085bbdee03b23 100644 Binary files a/ws_nodes_py/__pycache__/regulator.cpython-310.pyc and b/ws_nodes_py/__pycache__/regulator.cpython-310.pyc differ diff --git a/ws_nodes_py/regulator.py b/ws_nodes_py/regulator.py index 874acfccb2624145b5376b5668e4cdbee074eaed..341e55e7b7f68ddc869737087b0b8dc0c7015dd2 100644 --- a/ws_nodes_py/regulator.py +++ b/ws_nodes_py/regulator.py @@ -55,11 +55,24 @@ class Controller(GetParameterNode): self.__move_to_point(self.goal) elif self.type_move == "MOVE CIRCLE": self.__move_around(self.goal) + elif self.type_move == "MOVE HEADING": + self.__move_heading(self.goal) elif self.type_move == "STOP": self.__move_by_twist(0, 0) else: self.__move_by_twist(0, 0) + def __move_heading(self, goal): + target_heading = goal.z + err_angle = self.__angle_error(target_heading, self.heading) + ang = self.__get_angular_speed(err_angle) + + if abs(err_angle) < 5: + lin = goal.x + else: + lin = 0 + self.__move_by_twist(lin, ang) + def __move_around(self, point: Point): target_dist = point.z diff --git a/ws_nodes_py/settings/mission.py b/ws_nodes_py/settings/mission.py index 70847590994019b4d16e7a516ceb569966dd9df4..8488c3cc811c3ce2edbf2c2408932ef4e9bb6419 100644 --- a/ws_nodes_py/settings/mission.py +++ b/ws_nodes_py/settings/mission.py @@ -1,27 +1,13 @@ from ws_nodes_py.settings.robot_settings import is_real -from ws_nodes_py.settings.for_world import world_markers from ws_nodes_py.StateMachine.MotionStates import * from ws_nodes_py.StateMachine.SpecialStates import * from ws_nodes_py.StateMachine.DefaultStates import * -# TEST MISSIONS - -is_real_compass = True -square_mission = [ - [MoveToGoal, {"goal": [0, 2], "dist_treshold": 0.3, "navigation_marker": "", "is_real_compass": is_real_compass, "is_update_pos": True}], - [MoveToGoal, {"goal": [2, 2], "dist_treshold": 0.3}], - [MoveToGoal, {"goal": [2, 0], "dist_treshold": 0.3}], - [MoveToGoal, {"goal": [0, 0], "dist_treshold": 0.3}], -] - -circle_mission = [ - [MoveByCircle, {"goal": [10, 0], "dist": 1, "out_angle": 120, "is_real_compass": is_real_compass, "is_update_pos": True}] -] - on_navigation = [ [ [WorldSettings, {"is_real_compass": True, "is_update_pos": True}], + [MoveByTwist, {"goal": [0, 0]}], [Wait, {"time": 2}], ], ] @@ -29,71 +15,136 @@ on_navigation = [ off_navigation = [ [ [WorldSettings, {"is_real_compass": False, "is_update_pos": False}], + [MoveByTwist, {"goal": [0, 0]}], [Wait, {"time": 2}], ], ] +timer_thrust = 0.2 +is_anticlockwise = True -# FIRST STEP OF VLAD24 MISSION +def set_course(tar_course:int, is_clockwise:bool=False, thrust:float=None): + thrust = timer_thrust if thrust is None else thrust + thrust = -thrust if is_clockwise else thrust + return [ + [ + [MoveByTwist, {"goal": [0, thrust]}], + [IsAchieveHeading, {"target_angle": tar_course}], + ] + ] -def trough_gate(color: str): + +def move_by_time(goal:list, time:int): return [ - [ # Двигаемся к воротам пока не увидим, что шар ушёл в корму - [Wait, {"time": 100}], - [MoveToGoal, {"goal": f"{color}_gate", "navigation_marker": f"{color}_gate", "is_real_compass": False, "is_update_pos": False}], - [FindObject, {"object_name": f"{color}_bou", "min_max_angle": [0, 360], "time": 2}], - ], [ - [MoveByTwist, {"goal": [0.10, 0]}], - [FindObject, {"object_name": f"{color}_bou", "min_max_angle": [90, 270], "time": 2}], + [ + [MoveByTwist, {"goal": goal}], + [Wait, {"time": time}], ] ] + +def move_forward_by_time(time:int, thrust:float=None): + thrust = timer_thrust if thrust is None else thrust + return move_by_time([thrust, 0], time) + + +def get_clock_ang(ang): + if is_anticlockwise: + return (360 - ang) % 360 + else: + return ang + + +def inv_clock(val): + if is_anticlockwise: + return -val + else: + return val + + +# FIRST STEP OF VLAD24 MISSION + move_to_blue_gate = [ - # [ # Выезжаем из жёлтых ворот - # [MoveByTwist, {"goal": [0.15, 0]}], - # [Wait, {"time": 7}], - # ], [ # Ищем синий шар - [MoveByTwist, {"goal": [0.10, 0.075 if is_real else 0.13]}], + [MoveByTwist, {"goal": [0.10, inv_clock(0.075 if is_real else 0.13)]}], [FindObject, {"object_name": "blue_bou"}], ], [ # Ждём пока он уйдёт в корму - [MoveByTwist, {"goal": [0.10, -0.075 if is_real else -0.1]}], + [MoveByTwist, {"goal": [0.10, inv_clock(-0.075 if is_real else -0.1)]}], [FindObject, {"object_name": "blue_bou", "min_max_angle": [90, 270]}], - ], - # [ # Проезжаем чуть вперёд, чтобы выйти +- перпендикулярно воротам - # [MoveByTwist, {"goal": [0.15, 0]}], - # [Wait, {"time": 7}], - # ], - [ # Разворачиваемся к воротам - [MoveByTwist, {"goal": [0.0, -0.15]}], + ], [ # Разворачиваемся к воротам + [MoveByTwist, {"goal": [0.0, inv_clock(-0.15)]}], [FindObject, {"object_name": "blue_gate", "min_max_angle": [0, 360]}], ] - ] move_to_yellow_gate = [ [ # Поворочиваемся вокруг буя, пока не увидим жёлтые ворота # [MoveByCircle, {"goal": "blue_bou", "navigation_marker": "blue_bou"}], - [MoveByTwist, {"goal": [0.05 if is_real else 0.02, -0.17]}], + [MoveByTwist, {"goal": [0.05 if is_real else 0.02, inv_clock(-0.17)]}], [FindObject, {"object_name": "yellow_gate", "min_max_angle": [0, 360], "time": 2}], ] - # [ # Пока не видим 2 шара в боковой камере - # [MoveByTwist, {"goal": [0.15, 0.00]}], - # [FindObject, {"object_name": "yellow_bou_1", "min_max_angle": [0, 360]}], - # ], [ # Проежаем чуть вперёд, чтобы отойти от синих шаров - # [MoveByTwist, {"goal": [0.15, -0.05]}], - # [Wait, {"time": 5}], - # ], [ # Поворочиваемся лицом к воротам - # [MoveByTwist, {"goal": [0.0, -0.15]}], - # [FindObject, {"object_name": "yellow_gate", "min_max_angle": [0, 360]}], - # ] ] -first_step = off_navigation + trough_gate("yellow") + move_to_blue_gate + trough_gate("blue") + move_to_yellow_gate + trough_gate("yellow") +move_trouth_yellow_gate_extrapolations = [ + [ # Поворочиваемся вокруг буя, пока не увидим жёлтые ворота + [MoveByTwist, {"goal": [0.05 if is_real else 0.02, inv_clock(-0.17)]}], + [FindObject, {"object_name": "yellow_gate", "min_max_angle": [0, 360], "time": 2}], + [IsAchieveHeading, {"target_angle": 170}], + ], [ + [MoveByTwist, {"goal": [0.10, 0]}], + [FindObject, {"object_name": "yellow_bou", "min_max_angle": [90, 270], "time": 2}], + ] +] + +def trough_gate(color: str): + return [ + [ # Двигаемся к воротам пока не увидим, что шар ушёл в корму + [MoveToGoal, {"goal": f"{color}_gate", "navigation_marker": f"{color}_gate", "is_real_compass": False, "is_update_pos": False}], + [FindObject, {"object_name": f"{color}_bou", "min_max_angle": [0, 360], "time": 2}], + ], [ + [MoveByTwist, {"goal": [0.10, 0]}], + [FindObject, {"object_name": f"{color}_bou", "min_max_angle": [90, 270], "time": 2}], + ] + ] + +move_by_nav = [ + *on_navigation, + *set_course(20, True), + # TODO + [ + [MoveByHeading, {"tar_heading": 20, "thrust": 0.2}], + # [MoveByTwist, {"goal": [timer_thrust, 0]}], + [FindObject, {"object_name": "blue_bou", "min_max_angle": [140, 220], "time": 2}], + [Wait, {"time": 40}], # TODO remove + ], + *set_course(260, False), + [ + [MoveByHeading, {"tar_heading": 270, "thrust": 0.2}], + # [MoveByTwist, {"goal": [timer_thrust, 0]}], + [Wait, {"time": 20}], # TODO remove + ], [ # Поворочиваемся вокруг буя, пока не увидим жёлтые ворота + [MoveByTwist, {"goal": [0, inv_clock(-0.2)]}], + [FindObject, {"object_name": "yellow_gate", "min_max_angle": [0, 360], "time": 2}], + ], + *trough_gate("yellow"), +] +first_step = off_navigation + trough_gate("yellow") + move_to_blue_gate + trough_gate("blue") + move_to_yellow_gate + trough_gate("yellow") +first_step_nav = trough_gate("yellow") + move_by_nav +first_step_short = [ + *trough_gate("yellow"), + [ + [MoveByTwist, {"goal": [0.2, 0]}], + [Wait, {"time": 5 if is_real else 10}], + ], [ # Поворочиваемся вокруг буя, пока не увидим жёлтые ворота + [MoveByTwist, {"goal": [0, inv_clock(-0.2)]}], + [FindObject, {"object_name": "yellow_gate", "min_max_angle": [0, 360], "time": 2}], + ], + *trough_gate("yellow"), +] -# SECOND AND THIRD STEP OF VLAD24 MISSION +# SECOND OF VLAD24 MISSION def get_drop_cube(): if is_real: @@ -114,80 +165,24 @@ def get_drop_cube(): [[DropBox, {"time": 5}], [MoveByTwist, {"goal": [0, 0]}]] ] - -move_to_black_island_1 = [ - # TODO время подобранно для симулятора - # WARN в реальности это не прокатит - - # TODO отсюда экстраполируем своё положение (подставляем координаты жёлтых ворот) - # WARN если включаем экстраполяцию положения, то не стоит одновременно использовать linear и angular - # WARN если используем виртуальный компас, то лучше использовать константную угловую скорость 0.15 (отключить пиды) - - # TODO Общий таймер на этап, если сделаем прохождение сбора шаров - # (надо придумать, как это можно сделать) - [ # Отходим от ворот - [MoveByTwist, {"goal": [0.15, 0]}], - [Wait, {"time": 10}], - ], [ # Поворачиваем на "90" градусов - [MoveByTwist, {"goal": [0.0, 0.15]}], - [Wait, {"time": 22}], - # TODO выход по курсу - ], [ # Проходим ещё чуть вперёд - [MoveByTwist, {"goal": [0.15, 0]}], - [Wait, {"time": 20}], - ], [ # Поворачиваемся в сторону острова - [MoveByTwist, {"goal": [0.0, 0.15]}], - # [Wait, {"time": 25}], - [IsAchieveHeading, {"target_angle": 0}], - ], [ # Идём вперёд, пока не увидим чёрный круг - [MoveByTwist, {"goal": [0.15, 0]}], - [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], - ], -] - -set_course_to_black_island = [ +move_and_drop_cube = [ *on_navigation, + [DropZabralo, {"is_close": False}], [ # Отходим от ворот - [MoveByTwist, {"goal": [0.02, 0.15]}], + [MoveByTwist, {"goal": [0.05 if is_real else 0.02, 0.17]}], # [Wait, {"time": 45}], - [IsAchieveHeading, {"target_angle": 0}], + [IsAchieveHeading, {"target_angle": 60 if is_real else 0}], # TODO check + ], [ # Идём вперёд, пока не увидим чёрный круг + [MoveByHeading, {"tar_heading": 60 if is_real else 0, "thrust": 0.15}], + [FindObject, {"object_name": "black_position_front", "min_max_angle": [0, 360]}], ], - *off_navigation, + *get_drop_cube(), ] -move_to_black_island_2 = set_course_to_black_island + [ - # TODO время подобранно для симулятора - # WARN в реальности это не прокатит +second_step = move_and_drop_cube - # TODO отсюда экстраполируем своё положение (подставляем координаты жёлтых ворот) - # WARN если включаем экстраполяцию положения, то не стоит одновременно использовать linear и angular - # WARN если используем виртуальный компас, то лучше использовать константную угловую скорость 0.15 (отключить пиды) - - # TODO Общий таймер на этап, если сделаем прохождение сбора шаров - # (надо придумать, как это можно сделать) - [ # Идём вперёд, пока не увидим чёрный круг - [MoveByTwist, {"goal": [0.15, 0]}], - [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], - ], -] -move_around_black_island = [ - [ # Отходим от острова - [MoveByTwist, {"goal": [-0.15, 0]}], - [Wait, {"time": 3}], - ], [ # Разворачиваемся перпендикулярно острову - [MoveByTwist, {"goal": [0, 0.15]}], - [FindObject, {"object_name": "black_position", "min_max_angle": [90, 110]}], - ], - [ # Движемся вокруг острова - [MoveByCircle, {"goal": "black_position", "navigation_marker": "black_position", "is_real_compass": False, "is_update_pos": False}], - [Wait, {"time": 120}], - # Если шар под кругом, то через него мы не идём - # [FindObject, {"object_name": "red_position", "min_max_angle": [180, 360]}], - # Для теста смотрим только спереди - [FindObject, {"object_name": "red_position", "min_max_angle": [270, 20]}], - ], -] +# THIRD OF VLAD24 MISSION around_island_in_real = [ [ @@ -195,12 +190,12 @@ around_island_in_real = [ [Wait, {"time": 5}], ], [ - [MoveByTwist, {"goal": [0.01, 0.15]}], + [MoveByTwist, {"goal": [0.01, inv_clock(0.15)]}], # [Wait, {"time": 12}], - [FindObject, {"object_name": "black_position_side", "min_max_angle": [330, 90]}], + [FindObject, {"object_name": "black_position_side", "min_max_angle": [270, 90]}], ], [ - [MoveByTwist, {"goal": [0.03, -0.15]}], + [MoveByTwist, {"goal": [0.03, inv_clock(-0.15)]}], [Wait, {"time": 90}], [FindObject, {"object_name": "red_position", "min_max_angle": [270, 20]}], ], @@ -218,62 +213,80 @@ drop_stakan = [ [DropPickStakan, {"time": 15, "state": 1, "is_wait_pickup": True}], [MoveByTwist, {"goal": [0, 0]}] ], [ - [DropPickStakan, {"time": 40, "state": 0}], + [DropPickStakan, {"time": 40, "state": 0, "is_wait_pickup": True}], [MoveByTwist, {"goal": [0, 0]}] ] ] -second_step = move_to_black_island_2 + get_drop_cube() + (move_around_black_island if not is_real else around_island_in_real) + move_to_red_point + drop_stakan +third_step = [] +# third_step += around_island_in_real +third_step += move_to_red_point + drop_stakan -# THIRD STEP OF VLAD24 MISSION -set_to_course = [ +# FOURTH STEP OF VLAD24 MISSION + +move_to_red_poster = [ *on_navigation, + [DropZabralo, {"is_close": False}], [ [MoveByTwist, {"goal": [0, 0.15]}], - [IsAchieveHeading, {"target_angle": 155}], - ], - *off_navigation, -] - -move_to_balls = [ - [ - [MoveByTwist, {"goal": [0.10, 0]}], + [IsAchieveHeading, {"target_angle": 170 if is_real else 160}], # TODO + ], [ # Идём вперёд, пока не увидим красный плакат + [MoveByHeading, {"tar_heading": 170 if is_real else 160, "thrust": 0.15}], [FindObject, {"object_name": "red_poster_front", "min_max_angle": [0, 360], "time": 1}], ], [ [MoveToGoal, {"goal": "red_poster", "navigation_marker": "red_poster_front", "is_real_compass": False, "is_update_pos": False}], [FindObject, {"object_name": "red_poster_front", "min_max_angle": [340, 20], "max_dist": 0.5}], - [GetPostersDirection, {}], + # [GetPostersDirection, {}], ] ] balls_collecting = [ [ - [MoveByTwist, {"goal": [0.03, 0.2]}], - [FindObject, {"object_name": "red_poster_side", "min_max_angle": [80, 230]}], - ], - [ - [MoveByTwist, {"goal": [0.03, 0.2]}], + [MoveByTwist, {"goal": [0.03, -0.2]}], + [FindObject, {"object_name": "red_poster_side", "min_max_angle": [get_clock_ang(80), get_clock_ang(230)]}], + ], [ + [MoveByTwist, {"goal": [0.03, -0.2]}], [Wait, {"time": 15}], ] ] balls_delivery = [ + [DropZabralo, {"is_close": True}], [ - [MoveByVariableTwist, {"var": "move_side", "right": [0.08, 0.2], "left": [0.08, -0.2]}], + [MoveByTwist, {"goal": [0.03, 0.2]}], + # [MoveByVariableTwist, {"var": "move_side", "right": [0.08, 0.2], "left": [0.08, inv_clock(-0.2)]}], [FindObject, {"object_name": "green_poster", "min_max_angle": [320, 60]}], ], [ - [MoveByVariableTwist, {"var": "move_side", "right": [0.08, 0.2], "left": [0.08, -0.2]}], + [MoveByTwist, {"goal": [0.03, 0.2]}], + # [MoveByVariableTwist, {"var": "move_side", "right": [0.08, 0.2], "left": [0.08, inv_clock(-0.2)]}], [Wait, {"time": 10}], ] ] -third_step = set_to_course + move_to_balls + balls_collecting + balls_delivery +fourth_step = move_to_red_poster + balls_collecting + balls_delivery +fourth_step_short = [ + *on_navigation, + *set_course(180, False), + [ + [MoveByHeading, {"tar_heading": 180, "thrust": 0.2}], + # [MoveByTwist, {"goal": [0.2, 0]}], + [Wait, {"time": 150}], # TODO remove + ], +] # финальная миссия -# mission = first_step + second_step + third_step +# mission = first_step_short + second_step + third_step + fourth_step + +# Отдельные этапы для отработки +# mission = first_step_short + second_step + third_step + fourth_step_short +# mission = first_step_nav + second_step + third_step + fourth_step_short +mission = first_step + second_step + third_step + fourth_step_short +# mission = second_step +# mission = third_step +# mission = fourth_step """ 0) Для всех этапов надо записать БЭКИ @@ -288,7 +301,8 @@ third_step = set_to_course + move_to_balls + balls_collecting + balls_delivery 2) Отработка прохождения первого этапа """ # mission = first_step # проход через жёлтые, через синие, через жёлтые -# mission = trough_gate("blue") + move_to_yellow_gate + trough_gate("yellow") # через синие, через жёлтые +# mission = trough_gate("blue") + move_to_yellow_gate_anticlockwise + trough_gate("yellow") # через синие, через жёлтые +# mission = trough_gate("blue") + on_navigation + move_trouth_yellow_gate_anticlockwise_extrapolations # через синие, через жёлтые # mission = trough_gate("yellow") # отдельный проход через жёлтые # mission = trough_gate("blue") # отдельно проход через синие @@ -302,7 +316,7 @@ third_step = set_to_course + move_to_balls + balls_collecting + balls_delivery 4) Проверить выход на цель (правильность границ срабатывания) 5) Проверить выполнение второго этапа """ -# mission = second_step # выход к месту предполагаемого нахождения нефтяного пятна + сброс куба + сброс пробоотборника +# mission = second_step # выход к месту предполагаемого нахождения нефтяного пятна + сброс куба # mission = [[DropBox, {"time": 5}],] # сборс куба # mission = drop_stakan # сборс стакана # mission = set_course_to_black_island # выход на курс предполагаемого нахождения чёрного круга @@ -321,45 +335,137 @@ third_step = set_to_course + move_to_balls + balls_collecting + balls_delivery # mission = move_to_balls + balls_collecting # отладка сбора шаров # mission = move_to_balls + balls_collecting + balls_delivery # отладка сбора шаров и доставки в зону разгрузки +# mission = miss -# mission = trough_gate("blue") -# mission = first_step -# mission = second_step -# mission = move_around_black_island -# mission = move_to_yellow_gate +""" +mission = trough_gate("blue") +mission = first_step +mission = second_step +mission = move_around_black_island +mission = move_to_yellow_gate -# mission = get_drop_cube() # отладка сброса куба -# mission = move_to_red_point # отладка выхода на красный круг -# mission = move_to_red_point + drop_stakan # отладка выхода на красный круг со сбросом +mission = get_drop_cube() # отладка сброса куба +mission = move_to_red_point # отладка выхода на красный круг +mission = move_to_red_point + drop_stakan # отладка выхода на красный круг со сбросом -# mission = move_to_red_point +mission = move_to_red_point -# mission = move_to_balls + balls_collecting + balls_delivery -# mission = balls_collecting + balls_delivery -# mission = second_step -# balls_delivery +mission = move_to_balls + balls_collecting + balls_delivery +mission = balls_collecting + balls_delivery +mission = second_step +balls_delivery + + +mission = first_step + second_step + third_step + +mission = [ + [ + [MoveByTwist, {"goal": [0.15, 0]}], + [FindObject, {"object_name": "black_position_front", "min_max_angle": [0, 360]}], + ]] + get_drop_cube() + +mission = around_island_in_real + move_to_red_point + drop_stakan + +mission = [[ + [MoveByTwist, {"goal": [0.15, 0]}], + [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], + ]]+ get_drop_cube() + around_island_in_real + move_to_red_point + drop_stakan + +mission = [ + [DropZabralo, {"is_close": False}], + [Wait, {"time": 5}], + [DropZabralo, {"is_close": True}], +] + +mission = move_to_balls + balls_collecting + [[DropZabralo, {"is_close": True}],] + balls_delivery + +move_to_black_island_1 = [ + # TODO_ время подобранно для симулятора + # WARN в реальности это не прокатит + + # TODO_ отсюда экстраполируем своё положение (подставляем координаты жёлтых ворот) + # WARN если включаем экстраполяцию положения, то не стоит одновременно использовать linear и angular + # WARN если используем виртуальный компас, то лучше использовать константную угловую скорость 0.15 (отключить пиды) + + # TODO_ Общий таймер на этап, если сделаем прохождение сбора шаров + # (надо придумать, как это можно сделать) + [ # Отходим от ворот + [MoveByTwist, {"goal": [0.15, 0]}], + [Wait, {"time": 10}], + ], [ # Поворачиваем на "90" градусов + [MoveByTwist, {"goal": [0.0, 0.15]}], + [Wait, {"time": 22}], + # TODO_ выход по курсу + ], [ # Проходим ещё чуть вперёд + [MoveByTwist, {"goal": [0.15, 0]}], + [Wait, {"time": 20}], + ], [ # Поворачиваемся в сторону острова + [MoveByTwist, {"goal": [0.0, 0.15]}], + # [Wait, {"time": 25}], + [IsAchieveHeading, {"target_angle": 0}], + ], [ # Идём вперёд, пока не увидим чёрный круг + [MoveByTwist, {"goal": [0.15, 0]}], + [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], + ], +] -# mission = first_step + second_step + third_step +# TEST MISSIONS -# mission = [ -# [ -# [MoveByTwist, {"goal": [0.15, 0]}], -# [FindObject, {"object_name": "black_position_front", "min_max_angle": [0, 360]}], -# ]] + get_drop_cube() +is_real_compass = True +square_mission = [ + [MoveToGoal, {"goal": [0, 2], "dist_treshold": 0.3, "navigation_marker": "", "is_real_compass": is_real_compass, "is_update_pos": True}], + [MoveToGoal, {"goal": [2, 2], "dist_treshold": 0.3}], + [MoveToGoal, {"goal": [2, 0], "dist_treshold": 0.3}], + [MoveToGoal, {"goal": [0, 0], "dist_treshold": 0.3}], +] -# mission = around_island_in_real + move_to_red_point + drop_stakan +circle_mission = [ + [MoveByCircle, {"goal": [10, 0], "dist": 1, "out_angle": 120, "is_real_compass": is_real_compass, "is_update_pos": True}] +] -# mission = [[ -# [MoveByTwist, {"goal": [0.15, 0]}], -# [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], -# ]]+ get_drop_cube() + around_island_in_real + move_to_red_point + drop_stakan -# mission = [ -# [DropZabralo, {"is_close": False}], -# [Wait, {"time": 5}], -# [DropZabralo, {"is_close": True}], -# ] +set_course_to_black_island = [ + *on_navigation, + [ # Отходим от ворот + [MoveByTwist, {"goal": [0.02, 0.15]}], + # [Wait, {"time": 45}], + [IsAchieveHeading, {"target_angle": 80}], # TODO_ + ], + *off_navigation, +] + +move_to_black_island_2 = set_course_to_black_island + [ + # TODO_ время подобранно для симулятора + # WARN в реальности это не прокатит + + # TODO_ отсюда экстраполируем своё положение (подставляем координаты жёлтых ворот) + # WARN если включаем экстраполяцию положения, то не стоит одновременно использовать linear и angular + # WARN если используем виртуальный компас, то лучше использовать константную угловую скорость 0.15 (отключить пиды) + + # TODO_ Общий таймер на этап, если сделаем прохождение сбора шаров + # (надо придумать, как это можно сделать) + [ # Идём вперёд, пока не увидим чёрный круг + [MoveByTwist, {"goal": [0.15, 0]}], + [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], + ], +] -# mission = move_to_balls + balls_collecting + [[DropZabralo, {"is_close": True}],] + balls_delivery \ No newline at end of file +move_around_black_island = [ + [ # Отходим от острова + [MoveByTwist, {"goal": [-0.15, 0]}], + [Wait, {"time": 3}], + ], [ # Разворачиваемся перпендикулярно острову + [MoveByTwist, {"goal": [0, inv_clock(0.15)]}], + [FindObject, {"object_name": "black_position", "min_max_angle": [get_clock_ang(90), get_clock_ang(110)]}], + ], + [ # Движемся вокруг острова + [MoveByCircle, {"goal": "black_position", "navigation_marker": "black_position", "is_real_compass": False, "is_update_pos": False}], + [Wait, {"time": 120}], + # Если шар под кругом, то через него мы не идём + # Для теста смотрим только спереди + [FindObject, {"object_name": "red_position", "min_max_angle": [270, 20]}], + ], +] +""" \ No newline at end of file