Центрирование по нижней камере + мусор

Author: SanctaTinea

src/sauvc_launch/launch/control.launch.py

@@ -28,6 +28,9 @@ def generate_launch_description():
     bbox_centering_twist_action_arg = DeclareLaunchArgument(
         "bbox_centering_twist_action", default_value='/stingray/actions/bbox_centering_twist'
     )
+    bbox_bottom_centering_twist_action_arg = DeclareLaunchArgument(
+        "bbox_bottom_centering_twist_action", default_value='/stingray/actions/bbox_bottom_centering_twist'
+    )
     bbox_search_twist_action_arg = DeclareLaunchArgument(
         "bbox_search_twist_action", default_value='/stingray/actions/bbox_search_twist'
     )
@@ -75,6 +78,7 @@ def generate_launch_description():
         zbar_topic_arg,
         twist_action_arg,
         bbox_centering_twist_action_arg,
+        bbox_bottom_centering_twist_action_arg,
         hydroacoustic_centering_twist_action_arg,
         bbox_search_twist_action_arg,
         uv_state_topic_arg,
@@ -97,6 +101,7 @@ def generate_launch_description():
                 {'transition_srv': LaunchConfiguration("transition_srv")},
                 {'twist_action': LaunchConfiguration("twist_action")},
                 {'bbox_centering_twist_action': LaunchConfiguration("bbox_centering_twist_action")},
+                {'bbox_bottom_centering_twist_action': LaunchConfiguration("bbox_bottom_centering_twist_action")},
                 {'bbox_search_twist_action': LaunchConfiguration("bbox_search_twist_action")},
                 {'hydroacoustic_centering_twist_action': LaunchConfiguration("hydroacoustic_centering_twist_action")},
                 {'device_action': LaunchConfiguration("device_action")},
@@ -147,6 +152,18 @@ def generate_launch_description():
             respawn=True,
             respawn_delay=1,
         ),
+        Node(
+            package='stingray_movement',
+            executable='bbox_bottom_centering_twist_action_server',
+            name='bbox_bottom_centering_twist_action_server',
+            parameters=[
+                {'bbox_bottom_centering_twist_action': LaunchConfiguration("bbox_bottom_centering_twist_action")},
+                {'uv_state_topic': LaunchConfiguration("uv_state_topic")},
+                {'set_twist_srv': LaunchConfiguration("set_twist_srv")},
+            ],
+            respawn=True,
+            respawn_delay=1,
+        ),
         Node(
             package='stingray_movement',
             executable='bbox_search_twist_action_server',

src/sauvc_launch/launch/missions.launch.py

@@ -40,6 +40,9 @@ def generate_launch_description():
     bbox_centering_twist_action_arg = DeclareLaunchArgument(
         "bbox_centering_twist_action", default_value='/stingray/actions/bbox_centering_twist'
     )
+    bbox_bottom_centering_twist_action_arg = DeclareLaunchArgument(
+        "bbox_bottom_centering_twist_action", default_value='/stingray/actions/bbox_bottom_centering_twist'
+    )
     bbox_search_twist_action_arg = DeclareLaunchArgument(
         "bbox_search_twist_action", default_value='/stingray/actions/bbox_search_twist'
     )
@@ -86,6 +89,7 @@ def generate_launch_description():
         enable_object_detection_topic_arg,
         twist_action_arg,
         bbox_centering_twist_action_arg,
+        bbox_bottom_centering_twist_action_arg,
         hydroacoustic_centering_twist_action_arg,
         bbox_search_twist_action_arg,
         device_action_arg,
@@ -107,6 +111,7 @@ def generate_launch_description():
                 {'transition_srv': LaunchConfiguration("transition_srv")},
                 {'twist_action': LaunchConfiguration("twist_action")},
                 {'bbox_centering_twist_action': LaunchConfiguration("bbox_centering_twist_action")},
+                {'bbox_bottom_centering_twist_action': LaunchConfiguration("bbox_bottom_centering_twist_action")},
                 {'bbox_search_twist_action': LaunchConfiguration("bbox_search_twist_action")},
                 {'hydroacoustic_centering_twist_action': LaunchConfiguration("hydroacoustic_centering_twist_action")},
                 {'device_action': LaunchConfiguration("device_action")},

src/sauvc_missions/configs/missions/blue_bowl.yaml

@@ -17,7 +17,7 @@ states:
       avoid_horizontal_threshold: 0.0
       surge: 50.0
       sway: 0.0
-      depth: 1.5
+      depth: 0.3
       roll: 0.0
       pitch: 0.0
       duration: 15.0

src/sauvc_missions/configs/missions/bowl_centering.yaml

@@ -0,0 +1,23 @@
+initial: ENABLE_OBJECT_DETECTION
+states:
+  ENABLE_OBJECT_DETECTION:
+    action:
+      type: EnableObjectDetection
+      camera_topic: /stingray/topics/camera/front
+      enable: true
+  BOWL_CENTERING:
+    action: 
+      type: BboxBottomCenteringTwist
+      bbox_name: blue_bowl
+      bbox_topic: /stingray/topics/camera/bottom/bbox_array
+      threshold_x: 2
+      threshold_y: 2
+      lost_threshold: 3000
+      surge: 20.0
+      sway: 20.0
+      depth: 0.3
+      duration: 60.0
+      centering_rate: 1
+transitions:
+  - { trigger: ok, source: ENABLE_OBJECT_DETECTION, dest: BOWL_CENTERING }
+  - { trigger: ok, source: BOWL_CENTERING, dest: OK }

src/sauvc_missions/configs/missions/gate.yaml

@@ -21,17 +21,17 @@ states:
       bbox_name: gate
       bbox_topic: /stingray/topics/camera/front/bbox_array
       distance_threshold: 1.5
-      lost_threshold: 30
+      lost_threshold: 300
       avoid_bbox_name_array: [orange_flare]
       avoid_distance_threshold: 2.0
       avoid_horizontal_threshold: 2.0
-      surge: 100.0
-      sway: -20.0
-      depth: 0.5
+      surge: 50.0
+      sway: -50.0
+      depth: 1.5
       roll: 0.0
       pitch: 0.0
       duration: 15.0
-      centering_rate: 0.5
+      centering_rate: 5
   MOVE_THROUGH_GATE:
     action: 
       type: Twist

src/sauvc_missions/configs/scenarios/blue_bowl.yaml

@@ -8,6 +8,10 @@ missions:
   MOVE_TO_BOWL:
     config_name: blue_bowl
     package_name: sauvc_missions
+  BOWL_CENTERING:
+    config_name: bowl_centering
+    package_name: sauvc_missions
 transitions:
   - { source: INIT, outcome: OK, dest: MOVE_TO_BOWL }
-  - { source: MOVE_TO_BOWL, outcome: OK, dest: OK }
+  - { source: MOVE_TO_BOWL, outcome: OK, dest: BOWL_CENTERING }
+  - { source: BOWL_CENTERING, outcome: OK, dest: OK }

src/sauvc_missions/configs/scenarios/bowl_centering.yaml

@@ -0,0 +1,14 @@
+initial: INIT
+ok_state_package_name: stingray_missions
+failed_state_package_name: stingray_missions
+missions:
+  INIT:
+    config_name: init
+    package_name: stingray_missions
+  BOWL_CENTERING:
+    config_name: bowl_centering
+    package_name: sauvc_missions
+
+transitions:
+  - { source: INIT, outcome: OK, dest: BOWL_CENTERING }
+  - { source: BOWL_CENTERING, outcome: OK, dest: OK }

src/sauvc_missions/sauvc_missions/action.py

@@ -4,6 +4,7 @@
 from stingray_utils.acyncio import AsyncActionClient
 from stingray_interfaces.action import TwistAction
 from stingray_interfaces.action import BboxCenteringTwistAction
+from stingray_interfaces.action import BboxBottomCenteringTwistAction
 from stingray_interfaces.action import BboxSearchTwistAction
 from sauvc_interfaces.action import HydroacousticCenteringTwistAction
 from stingray_missions.action import StateActionBase, load_stingray_actions