From 233871d559cc4888d96bdca43a14029aa82e0e36 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Sun, 20 Oct 2024 08:28:07 +0300 Subject: [PATCH] release --- ws_nodes_py/CVs/CVCamera.py | 4 +- ws_nodes_py/CVs/videoS.py | 2 +- ws_nodes_py/StateMachine/MotionStates.py | 31 +- ws_nodes_py/StateMachine/SpecialStates.py | 2 +- .../__pycache__/regulator.cpython-310.pyc | Bin 6627 -> 6976 bytes ws_nodes_py/regulator.py | 13 + ws_nodes_py/settings/mission.py | 454 +++++++++++------- 7 files changed, 325 insertions(+), 181 deletions(-) diff --git a/ws_nodes_py/CVs/CVCamera.py b/ws_nodes_py/CVs/CVCamera.py index 5faaace..602d6e6 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 c9478e3..807290b 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 d04da47..b3c7cb1 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 eff92aa..1be7925 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 GIT binary patch delta 1042 zcmY+D-%C_c5XaBl-|dglMkhDJ@tdlhlrqmB8nv>=plRv3VOGhyGeN9eD2KLxpTi~?%eMOKOVLVe!qv| zvOhQ7T;KD?PD9@#bTcdgoF$Y+mIV%6S+Ei+SGl&p5*pXJfmfZI86&H3i|<&_5+>h8 zKKEv7?r9>wH_MW(StV3;{A)t^wYU|CLjvPaD_yREDOFwTY<+KlUy ziinrl3UGrPtIF1$?Y?8mU!K^m8Q+D$(RgcL{LI<&1Lwp6)GYwvSaPqIx74D^okrG94SORsH8oKbVan=9IEILm-6ey8wj1hHE-#wNcM(CWW^X^-8H^QegaI}l z&Zk(QDFK*?{_jTD_9trkbRm~7r4{+oj#U462S69YSU|@8FPod z_DR?;hA_P0w!1;3(RTAhD_NHboUo5DijZF$x}7odb`!1;t`e>xYMzV`1(D>rVo4-0 zAbSHHP?Gt;+ji0+PE(ROeQ#naSG?tD^@dQQ`?%6r*bYhW27i!Tq;eMV7WTdGz~l5 znlvsyaO3)RN~GkMo!8--Jlxy~D>BvmB3Qv2?%M_ZQ4WY%*%CSeujNSSnV(wW;HXp1 z7|M^qnv8`HL8H7H-hc2Sg-yaBK@bKAG!XUn(pRbX-EH)&Tnl&GG!7Q?fz@W&wpacR nuQh+bP?3Id3k;kF3w$7!Haz6$Ti delta 676 zcmY+COK1~O6o&6f=H<*Jlb6Y)5?myqj7Y60BGOtx1Q(?x6l)z3vEICFZQ_jzwvC7c ziVHVAD?xnV!qSCQqli02*DghZh@c=@xpF7gdnPLn+>iPHx&N7a5A*iW_XE*JBw{jI znIB4ZJoh|$5l&v0vdnzo%nz-x3UJ_|j^T%Sm@6ITD_kwBl@Qms-cfvwyQKM!vR+ZR z$*qp+8{FoR4#PLF$6R9y^PL&yWxKzjZNM#TnU&qZRjxh`0po0jt+Ge|X-~t1mAA!y zc@6;*o{rNN$B$M~Pe^Hki+`;`w-#wYxChJlC7Q>3mV;02IGWBRl<>536DGQ!oTHHL zt1{9L+Bo72we%(Mgr-&0|UM4IMh7oopaVrsn6?~WIj&U-}68&vio+gfv zQ6-EKa)cm?TsPG}*z7gUPlrmL0Xz(lnS2Mj>kBRQA=*Eorpqr!znU6AZ9H XtfM>hRa>Y09sDyiuICNi1ZDdlCN7*_ diff --git a/ws_nodes_py/regulator.py b/ws_nodes_py/regulator.py index 874acfc..341e55e 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 7084759..8488c3c 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 -- GitLab