Исправил ошибку с флагами

Исправил ошибки с вёдрами
Добавил сценарий для только столбиков

Author: SanctaTinea

src/sauvc_missions/configs/missions/flares.yaml

@@ -22,7 +22,8 @@ states:
       max_yaw: 360
       yaw_step: 30.0
       surge: 30.0
-      avoid_sway: 0.0
+      avoid_sway: -50
+      sway: 0
       depth: 1.5
       roll: 0.0
       pitch: 0.0

src/sauvc_missions/configs/missions/hydroacoustic_bowl.yaml

@@ -28,8 +28,8 @@ states:
       bbox_name: red_bowl
       bbox_topic: /stingray/topics/front_camera/bbox_array
       hydroacoustic_topic: /stingray/topics/angle_hydroacoustic
-      angle_threshold: 10.0
-      distance_threshold: 2.0
+      angle_threshold: 2.0
+      distance_threshold: 1.2
       lost_threshold: 30
       surge: 50.0
       sway: 0.0
@@ -54,7 +54,7 @@ states:
       roll: 0.0
       pitch: 0.0
       yaw: 0.0
-      duration: 4.0
+      duration: 5.0
   DROPPER_OPEN:
     action:
       type: SetDeviceValue

src/sauvc_missions/configs/scenarios/only_flares.yaml

@@ -0,0 +1,16 @@
+initial: INIT
+ok_state_package_name: stingray_missions
+failed_state_package_name: stingray_missions
+abort_state_package_name: stingray_missions
+missions:
+  INIT:
+    config_name: init
+    package_name: stingray_missions
+  FLARES:
+    config_name: flares
+    package_name: sauvc_missions
+
+transitions:
+  - { source: INIT, outcome: OK, dest: FLARES }
+  - { source: FLARES, outcome: OK, dest: OK }
+  - { source: [INIT, FLARES], outcome: FAILED, dest: FAILED }

src/sauvc_movement/src/HydroacousticCenteringTwistActionServer.cpp

@@ -11,7 +11,7 @@ HydroacousticCenteringTwistActionServer::HydroacousticCenteringTwistActionServer
 void HydroacousticCenteringTwistActionServer::bboxArrayCallback(const stingray_interfaces::msg::BboxArray &msg) {
     bool found_target = false;
     for (auto bbox : msg.bboxes) {
-        if (bbox.name == target_bbox_name && abs(bbox.horizontal_angle) < target_angle_threshold && abs(bbox.horizontal_angle) < abs(current_target_bbox.horizontal_angle)) {
+        if (bbox.name == target_bbox_name) {//} && abs(bbox.horizontal_angle) < target_angle_threshold && abs(bbox.horizontal_angle) < abs(current_target_bbox.horizontal_angle)) {
             current_target_bbox = bbox;
             found_target = true;
             target_disappeared_counter = 0;
@@ -61,13 +61,6 @@ void HydroacousticCenteringTwistActionServer::execute(const std::shared_ptr<rclc
     auto goal_result = std::make_shared<sauvc_interfaces::action::HydroacousticCenteringTwistAction::Result>();
     goal_result->success = false;
 
-    bboxArraySub = _node->create_subscription<stingray_interfaces::msg::BboxArray>(
-        goal->bbox_topic, 10,
-        std::bind(&HydroacousticCenteringTwistActionServer::bboxArrayCallback, this, std::placeholders::_1));
-
-    angleHydroacousticSub = _node->create_subscription<std_msgs::msg::Float32>(goal->hydroacoustic_topic, 10,
-        std::bind(&HydroacousticCenteringTwistActionServer::hydroacousticCallback, this, std::placeholders::_1));
-
     // check duration
     if (goal->duration < 0.0) {
         goal_result->success = false;
@@ -76,6 +69,13 @@ void HydroacousticCenteringTwistActionServer::execute(const std::shared_ptr<rclc
         return;
     }
 
+    bboxArraySub = _node->create_subscription<stingray_interfaces::msg::BboxArray>(
+        goal->bbox_topic, 10,
+        std::bind(&HydroacousticCenteringTwistActionServer::bboxArrayCallback, this, std::placeholders::_1));
+
+    angleHydroacousticSub = _node->create_subscription<std_msgs::msg::Float32>(goal->hydroacoustic_topic, 10,
+        std::bind(&HydroacousticCenteringTwistActionServer::hydroacousticCallback, this, std::placeholders::_1));
+
     // send service request
     target_bbox_name = goal->bbox_name;
     target_distance_threshold = goal->distance_threshold;
@@ -102,18 +102,34 @@ void HydroacousticCenteringTwistActionServer::execute(const std::shared_ptr<rclc
 
         if (isTwistDone(goal) && isCenteringTwistDone()) {
             RCLCPP_INFO(_node->get_logger(), "Twist done, target distance: %f, closer than: %f", current_target_bbox.pos_z, target_distance_threshold);
+            twistSrvRequest->surge = 0;
+            //break;
+        }
+
+        if (isTwistDone(goal) && isCenteringTwistDone() && abs(current_uv_state.yaw - twistSrvRequest->yaw) < goal->angle_threshold) {
+            RCLCPP_INFO(_node->get_logger(), "Centering done, current yaw: %f, pinger agnle: %f", current_uv_state.yaw, current_hydroacoustic_angle);
+            twistSrvRequest->surge = 0;
             break;
         }
-        twistSrvRequest->yaw = current_hydroacoustic_angle;
+
+        twistSrvRequest->yaw = current_uv_state.yaw + current_hydroacoustic_angle;
         RCLCPP_INFO(_node->get_logger(), "Twist action current yaw: %f, request diff: %f, surge: %f", current_uv_state.yaw, twistSrvRequest->yaw, twistSrvRequest->surge);
+        RCLCPP_INFO(_node->get_logger(), "Pure current_hydroacoustic_angle: %f", current_hydroacoustic_angle);
         // check if service success
         twistSrvClient->async_send_request(twistSrvRequest).wait();
-        twistSrvRequest->yaw = 0.0;
+        // twistSrvRequest->yaw = 0.0;
 
         if (goal_handle->is_canceling()) {
             goal_result->success = false;
             RCLCPP_INFO(_node->get_logger(), "Goal canceled");
             goal_handle->canceled(goal_result);
+
+            target_disappeared_counter = 0;
+            target_bbox_name = "";
+            angleHydroacousticSub.reset();
+            bboxArraySub.reset();
+            // stop maneuvr service request
+            stopTwist(twistSrvRequest);
             return;
         }
         // rclcpp::spin_some(_node);