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

src/stingray_interfaces/CMakeLists.txt

@@ -14,6 +14,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/SetTransition.srv"
   "action/TwistAction.action"
   "action/BboxCenteringTwistAction.action"
+  "action/BboxBottomCenteringTwistAction.action"
   "action/BboxSearchTwistAction.action"
   "action/DeviceAction.action"
   "action/HydroacousticCenteringTwistAction.action"

src/stingray_interfaces/action/BboxBottomCenteringTwistAction.action

@@ -0,0 +1,21 @@
+string bbox_name 
+string bbox_topic
+float32 threshold_x
+float32 threshold_y
+int32 lost_threshold
+string[] avoid_bbox_name_array 
+float32 avoid_distance_threshold
+float32 avoid_horizontal_threshold
+float32 surge 
+float32 sway 
+float32 depth
+float32 roll
+float32 pitch
+float32 duration
+float32 centering_rate
+---
+# Result
+bool success
+---
+# Feedback
+std_msgs/Empty empty

src/stingray_launch/launch/control.launch.py

@@ -33,6 +33,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'
     )

src/stingray_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/stingray_missions/stingray_missions/action.py

@@ -6,6 +6,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 stingray_interfaces.action import HydroacousticCenteringTwistAction
 from stingray_interfaces.action import DeviceAction
@@ -349,6 +350,70 @@ async def execute(self,
                 f"Error while executing {self.node.get_parameter('bbox_centering_twist_action').get_parameter_value().string_value}")
             return False
         return await super().execute(**kwargs)
+
+class BboxBottomCenteringTwistStateAction(StateActionBase):
+    type = "BboxBottomCenteringTwist"
+
+    def __init__(self, node: Node):
+        super().__init__(node=node)
+
+        self.bbox_bottom_centering_twist_action_client = AsyncActionClient(
+            self.node, BboxBottomCenteringTwistAction, self.node.get_parameter('bbox_bottom_centering_twist_action').get_parameter_value().string_value)
+
+    def stop(self):
+        get_logger("action").info(
+            f"Stopping {self.type} action")
+        self.bbox_bottom_centering_twist_action_client.cancel()
+        return super().stop()
+
+    async def execute(self,
+                      bbox_name: str = "",
+                      bbox_topic: str = "",
+                      threshold_x: float = 0.2,
+                      threshold_y: float = 0.2,
+                      lost_threshold: int = 0,
+                      avoid_bbox_name_array: list[str] = [],
+                      avoid_distance_threshold: float = 0.0,
+                      avoid_horizontal_threshold: float = 0.0,
+                      surge: float = 0.0,
+                      sway: float = 0.0,
+                      depth: float = 0.0,
+                      roll: float = 0.0,
+                      pitch: float = 0.0,
+                      duration: float = 0.0,
+                      centering_rate: float = 0.0,
+                      **kwargs) -> bool:
+        get_logger("action").info(f"Executing {self.type} state action")
+
+        if not self.bbox_bottom_centering_twist_action_client.wait_for_server(timeout_sec=1.0):
+            get_logger("action").error(
+                f"Timeout while waiting for {self.bbox_bottom_centering_twist_action_client._action_name} action server")
+            return False
+
+        self.goal = BboxBottomCenteringTwistAction.Goal()
+        self.goal.bbox_name = bbox_name
+        self.goal.bbox_topic = bbox_topic
+        self.goal.threshold_x = float(threshold_x)
+        self.goal.threshold_y = float(threshold_y)
+        self.goal.lost_threshold = int(lost_threshold)
+        self.goal.avoid_bbox_name_array = avoid_bbox_name_array
+        self.goal.avoid_distance_threshold = float(avoid_distance_threshold)
+        self.goal.avoid_horizontal_threshold = float(
+            avoid_horizontal_threshold)
+        self.goal.surge = float(surge)
+        self.goal.sway = float(sway)
+        self.goal.depth = float(depth)
+        self.goal.roll = float(roll)
+        self.goal.pitch = float(pitch)
+        self.goal.duration = float(duration)
+        self.goal.centering_rate = float(centering_rate)
+
+        result = await self.bbox_bottom_centering_twist_action_client.send_goal_async(self.goal)
+        if not result.result.success:
+            get_logger('action').error(
+                f"Error while executing {self.node.get_parameter('bbox_bottom_centering_twist_action').get_parameter_value().string_value}")
+            return False
+        return await super().execute(**kwargs)
 
 
 class BboxSearchTwistStateAction(StateActionBase):
@@ -457,6 +522,7 @@ def load_stingray_actions(node: Node) -> dict[str, StateActionBase]:
         ThrusterIndicationStateAction.type: ThrusterIndicationStateAction(node),
         TwistStateAction.type: TwistStateAction(node),
         BboxCenteringTwistStateAction.type: BboxCenteringTwistStateAction(node),
+        BboxBottomCenteringTwistStateAction.type: BboxBottomCenteringTwistStateAction(node),
         BboxSearchTwistStateAction.type: BboxSearchTwistStateAction(node),
         SetDeviceValueStateAction.type: SetDeviceValueStateAction(node),
     }

src/stingray_missions/stingray_missions/fsm_node.py

@@ -40,6 +40,8 @@ def declare_parameters(node: Node):
         'bbox_search_twist_action', '/stingray/actions/bbox_search_twist')
     node.declare_parameter(
         'bbox_centering_twist_action', '/stingray/actions/bbox_centering_twist')
+    node.declare_parameter(
+        'bbox_bottom_centering_twist_action', '/stingray/actions/bbox_bottom_centering_twist')
     node.declare_parameter(
         'device_action', '/stingray/actions/device')
     node.declare_parameter(

src/stingray_movement/CMakeLists.txt

@@ -44,6 +44,15 @@ add_executable(bbox_centering_twist_action_server
         )
 ament_target_dependencies(bbox_centering_twist_action_server rclcpp rclcpp_action rclcpp_components std_msgs std_srvs stingray_core_interfaces stingray_interfaces stingray_utils)
 
+add_executable(bbox_bottom_centering_twist_action_server
+        include/stingray_movement/AbstractTwistActionServer.h
+        include/stingray_movement/AbstractBottomCenteringTwistActionServer.h
+        include/stingray_movement/BboxBottomCenteringTwistActionServer.h
+        src/BboxBottomCenteringTwistActionServer.cpp
+        )
+ament_target_dependencies(bbox_bottom_centering_twist_action_server rclcpp rclcpp_action rclcpp_components std_msgs std_srvs stingray_core_interfaces stingray_interfaces stingray_utils)
+
+
 add_executable(bbox_search_twist_action_server
         include/stingray_movement/AbstractTwistActionServer.h
         include/stingray_movement/AbstractSearchTwistActionServer.h
@@ -61,6 +70,7 @@ install(
 install(TARGETS
         twist_action_server
         bbox_centering_twist_action_server
+        bbox_bottom_centering_twist_action_server
         bbox_search_twist_action_server
         ${PROJECT_NAME}
         DESTINATION lib/${PROJECT_NAME}

src/stingray_movement/include/stingray_movement/AbstractBottomCenteringTwistActionServer.h

@@ -0,0 +1,44 @@
+#ifndef STINGRAY_SRC_STINGRAY_MOVEMENT_INCLUDE_ABSTRACTCENTERINGTWISTACTIONSERVER_H_
+#define STINGRAY_SRC_STINGRAY_MOVEMENT_INCLUDE_ABSTRACTCENTERINGTWISTACTIONSERVER_H_
+
+#include "stingray_movement/AbstractTwistActionServer.h"
+#include "stingray_core_interfaces/srv/set_twist.hpp"
+#include "stingray_core_interfaces/msg/uv_state.hpp"
+
+/**
+ * Action server that is responsible for moving vehicle
+ * by march and lag.
+ */
+template <class TBottomCenteringTwistAction, class TBottomCenteringTwistActionGoal>
+class AbstractBottomCenteringTwistActionServer : public AbstractTwistActionServer<TBottomCenteringTwistAction, TBottomCenteringTwistActionGoal> {
+
+protected:
+    virtual bool isCenteringTwistDone() = 0;
+    virtual bool isTargetLost() = 0;
+
+    float target_distance_threshold;
+    int target_lost_thresh;
+
+    int target_disappeared_counter = 0;
+
+public:
+
+    AbstractBottomCenteringTwistActionServer(std::shared_ptr<rclcpp::Node> _node, const std::string &actionName) : AbstractTwistActionServer<TBottomCenteringTwistAction, TBottomCenteringTwistActionGoal>(_node, actionName) {
+    // ROS subscribers
+        uvStateSub = _node->create_subscription<stingray_core_interfaces::msg::UVState>(
+            _node->get_parameter("uv_state_topic").as_string(), 1000,
+            std::bind(&AbstractBottomCenteringTwistActionServer::uvStateCallback, this, std::placeholders::_1));
+    };
+    ~AbstractBottomCenteringTwistActionServer() = default;
+
+    stingray_core_interfaces::msg::UVState current_uv_state;
+
+private:
+    void uvStateCallback(const stingray_core_interfaces::msg::UVState &msg) {
+        current_uv_state = msg;
+    }
+
+    rclcpp::Subscription<stingray_core_interfaces::msg::UVState>::SharedPtr uvStateSub;
+};
+
+#endif //STINGRAY_SRC_STINGRAY_MOVEMENT_INCLUDE_ABSTRACTCENTERINGTWISTACTIONSERVER_H_

src/stingray_movement/include/stingray_movement/BboxBottomCenteringTwistActionServer.h

@@ -0,0 +1,40 @@
+#ifndef STINGRAY_SRC_STINGRAY_MOVEMENT_INCLUDE_BBOXBOTTOMCENTERINGTWISTACTIONSERVER_H_
+#define STINGRAY_SRC_STINGRAY_MOVEMENT_INCLUDE_BBOXBOTTOMCENTERINGTWISTACTIONSERVER_H_
+
+#include <stingray_interfaces/action/bbox_bottom_centering_twist_action.hpp>
+
+#include "stingray_movement/AbstractBottomCenteringTwistActionServer.h"
+#include "stingray_utils/AsyncTimer.h"
+#include "stingray_interfaces/msg/bbox_array.hpp"
+
+#include <string.h>
+
+using namespace std::chrono_literals;
+
+/**
+ * Action server that is responsible for moving vehicle
+ * by march and lag.
+ */
+class BboxBottomCenteringTwistActionServer : public AbstractBottomCenteringTwistActionServer<stingray_interfaces::action::BboxBottomCenteringTwistAction, stingray_interfaces::action::BboxBottomCenteringTwistAction_Goal> {
+
+public:
+    BboxBottomCenteringTwistActionServer(std::shared_ptr<rclcpp::Node> _node, const std::string &actionName);
+    ~BboxBottomCenteringTwistActionServer() = default;
+
+private:
+    bool isTwistDone(const std::shared_ptr<const stingray_interfaces::action::BboxBottomCenteringTwistAction_Goal> goal)  override;
+    bool isCenteringTwistDone() override;
+    bool isTargetLost() override;
+    void bboxArrayCallback(const stingray_interfaces::msg::BboxArray &msg);
+    void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<stingray_interfaces::action::BboxBottomCenteringTwistAction>> goal_handle) override;
+    rclcpp::Subscription<stingray_interfaces::msg::BboxArray>::SharedPtr bboxArraySub;
+
+    std::string target_bbox_name = "";
+    std::vector<std::string> target_avoid_bbox_name_array;
+    stingray_interfaces::msg::Bbox current_target_bbox;
+    stingray_interfaces::msg::Bbox current_avoid_target_bbox;
+    float target_threshold_x;
+    float target_threshold_y;
+};
+
+#endif //STINGRAY_SRC_STINGRAY_MOVEMENT_INCLUDE_BBOXBOTTOMCENTERINGTWISTACTIONSERVER_H_