diff --git a/lesson_10/step_3.py b/lesson_10/step_3.py index 14a365b4051773370cc7298ae3144466765660b6..85261302c474a44bd53156678a4eb4ad14ca00e3 100644 --- a/lesson_10/step_3.py +++ b/lesson_10/step_3.py @@ -82,7 +82,8 @@ class Step(Node): ) self.declare_parameter( "allowed_classes", - ["red_bou", "yellow_bou"], # defaults + ["green_square"], + #["red_bou", "yellow_bou"], # defaults classes_descriptor ) @@ -317,7 +318,7 @@ class Step(Node): if len(data) < len(self.get_parameter("allowed_classes").value): return None - + """ # Сумма азимутов всех обнаруженных объектов azimuth_sum = sum([azimuth for _, azimuth, _ in data]) @@ -325,6 +326,13 @@ class Step(Node): course = azimuth_sum / len(data) return course + """ + + if not data: + return None + + return data[0][1] + def annotate_result_azimuth(self, annotated_frame: np.ndarray, azimuth: float) -> np.ndarray: """ diff --git a/lesson_10/step_4.py b/lesson_10/step_4.py index 8977aa68077130a967511e116d58ebbc8ddb97b6..cb88c5fde78256fb9609893333dbdc7c9ac91c14 100644 --- a/lesson_10/step_4.py +++ b/lesson_10/step_4.py @@ -17,18 +17,18 @@ class Step(Node): super().__init__("step_4") # Скорость поворота - range = FloatingPointRange(from_value=0.0, to_value=2.0, step=0.01) + range = FloatingPointRange(from_value=0.0, to_value=10.0, step=0.01) descriptor = ParameterDescriptor(floating_point_range=[range]) - self.declare_parameter("max_angular_speed", 0.15, descriptor) + self.declare_parameter("max_angular_speed", 0.2, descriptor) # Зона нечувствительности по угловой ошибке range = FloatingPointRange(from_value=0.0, to_value=180.0, step=0.01) descriptor = ParameterDescriptor(floating_point_range=[range]) - self.declare_parameter("dead_zone_angle", 5.0, descriptor) + self.declare_parameter("dead_zone_angle", 0.0, descriptor) range = FloatingPointRange(from_value=0.0, to_value=2.0, step=0.01) descriptor = ParameterDescriptor(floating_point_range=[range]) - self.declare_parameter("linear_speed", 0.5, descriptor) + self.declare_parameter("linear_speed", 0.8, descriptor) self.declare_parameter("is_enabled", True) diff --git a/lesson_10/step_5_state_machine.py b/lesson_10/step_5_state_machine.py index c7b35f251255a26e259c91627a879d1dcb7ed532..27d86cbd3a838cd159f5934353e7602821941870 100644 --- a/lesson_10/step_5_state_machine.py +++ b/lesson_10/step_5_state_machine.py @@ -49,9 +49,9 @@ def main(): add_state(SetMotionControllerIsEnabled(c, is_enabled=False), "stop_autopilot_motion", "move_forward") - add_state(PublishTwist(c, twist_list=(1.0, -0.05), duration_s=10), "move_forward", {"done": "search_gate"}) + add_state(PublishTwist(c, twist_list=(1.0, 0.0), duration_s=1), "move_forward", {"done": "search_gate"}) - add_state(SearchGate(c, angular_speed=-0.15), "search_gate", {"detect": "start_autopilot_motion"}) + add_state(SearchGate(c, angular_speed=0.15), "search_gate", {"detect": "start_autopilot_motion"}) add_state(SetMotionControllerIsEnabled(c, is_enabled=True), "start_autopilot_motion", "start")