diff --git a/src/stingray_core b/src/stingray_core index d38754b..d341467 160000 --- a/src/stingray_core +++ b/src/stingray_core @@ -1 +1 @@ -Subproject commit d38754b01063a3d67fc48e7f288c093b6bf9c236 +Subproject commit d341467ca5278c086d1b1ef2eb4c3a9036fa70f6 diff --git a/src/stingray_interfaces/CMakeLists.txt b/src/stingray_interfaces/CMakeLists.txt index 4c6548b..50ad5c6 100644 --- a/src/stingray_interfaces/CMakeLists.txt +++ b/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" diff --git a/src/stingray_interfaces/action/BboxBottomCenteringTwistAction.action b/src/stingray_interfaces/action/BboxBottomCenteringTwistAction.action new file mode 100644 index 0000000..4115532 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/stingray_launch/launch/control.launch.py b/src/stingray_launch/launch/control.launch.py index 88a3824..e9354f7 100644 --- a/src/stingray_launch/launch/control.launch.py +++ b/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' ) diff --git a/src/stingray_launch/launch/missions.launch.py b/src/stingray_launch/launch/missions.launch.py index e869fda..194efc3 100644 --- a/src/stingray_launch/launch/missions.launch.py +++ b/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")}, diff --git a/src/stingray_missions/stingray_missions/action.py b/src/stingray_missions/stingray_missions/action.py index afff0c4..8084895 100644 --- a/src/stingray_missions/stingray_missions/action.py +++ b/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), } diff --git a/src/stingray_missions/stingray_missions/fsm_node.py b/src/stingray_missions/stingray_missions/fsm_node.py index 1859d50..1a79794 100644 --- a/src/stingray_missions/stingray_missions/fsm_node.py +++ b/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( diff --git a/src/stingray_movement/CMakeLists.txt b/src/stingray_movement/CMakeLists.txt index 5c590f7..4c47ff8 100644 --- a/src/stingray_movement/CMakeLists.txt +++ b/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} diff --git a/src/stingray_movement/include/stingray_movement/AbstractBottomCenteringTwistActionServer.h b/src/stingray_movement/include/stingray_movement/AbstractBottomCenteringTwistActionServer.h new file mode 100644 index 0000000..e1adeb5 --- /dev/null +++ b/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 AbstractBottomCenteringTwistActionServer : public AbstractTwistActionServer { + +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 _node, const std::string &actionName) : AbstractTwistActionServer(_node, actionName) { + // ROS subscribers + uvStateSub = _node->create_subscription( + _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::SharedPtr uvStateSub; +}; + +#endif //STINGRAY_SRC_STINGRAY_MOVEMENT_INCLUDE_ABSTRACTCENTERINGTWISTACTIONSERVER_H_ diff --git a/src/stingray_movement/include/stingray_movement/BboxBottomCenteringTwistActionServer.h b/src/stingray_movement/include/stingray_movement/BboxBottomCenteringTwistActionServer.h new file mode 100644 index 0000000..d8b8e32 --- /dev/null +++ b/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 + +#include "stingray_movement/AbstractBottomCenteringTwistActionServer.h" +#include "stingray_utils/AsyncTimer.h" +#include "stingray_interfaces/msg/bbox_array.hpp" + +#include + +using namespace std::chrono_literals; + +/** + * Action server that is responsible for moving vehicle + * by march and lag. + */ +class BboxBottomCenteringTwistActionServer : public AbstractBottomCenteringTwistActionServer { + +public: + BboxBottomCenteringTwistActionServer(std::shared_ptr _node, const std::string &actionName); + ~BboxBottomCenteringTwistActionServer() = default; + +private: + bool isTwistDone(const std::shared_ptr goal) override; + bool isCenteringTwistDone() override; + bool isTargetLost() override; + void bboxArrayCallback(const stingray_interfaces::msg::BboxArray &msg); + void execute(const std::shared_ptr> goal_handle) override; + rclcpp::Subscription::SharedPtr bboxArraySub; + + std::string target_bbox_name = ""; + std::vector 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_ diff --git a/src/stingray_movement/src/BboxBottomCenteringTwistActionServer.cpp b/src/stingray_movement/src/BboxBottomCenteringTwistActionServer.cpp new file mode 100644 index 0000000..cc40b67 --- /dev/null +++ b/src/stingray_movement/src/BboxBottomCenteringTwistActionServer.cpp @@ -0,0 +1,232 @@ +#include "stingray_movement/BboxBottomCenteringTwistActionServer.h" + +BboxBottomCenteringTwistActionServer::BboxBottomCenteringTwistActionServer(std::shared_ptr _node, const std::string &actionName) : AbstractBottomCenteringTwistActionServer(_node, actionName) { + current_target_bbox.pos_x = 1000.0; + current_target_bbox.pos_y = 1000.0; + current_target_bbox.pos_z = 1000.0; + current_target_bbox.horizontal_angle = 1000.0; + current_target_bbox.vertical_angle = 1000.0; + current_avoid_target_bbox.pos_x = 1000.0; + current_avoid_target_bbox.pos_y = 1000.0; + current_avoid_target_bbox.pos_z = 1000.0; +}; + +void BboxBottomCenteringTwistActionServer::bboxArrayCallback(const stingray_interfaces::msg::BboxArray &msg) { + bool found_target = false; + bool found_avoid_target = false; + current_avoid_target_bbox.pos_x = 1000.0; + current_avoid_target_bbox.pos_y = 1000.0; + current_avoid_target_bbox.pos_z = 1000.0; + //current_target_bbox.horizontal_angle = 1000.0; + //current_target_bbox.vertical_angle = 1000.0; + + for (auto bbox : msg.bboxes) { + // RCLCPP_INFO(_node->get_logger(), "\nName x: %s", bbox.name.c_str()); + // RCLCPP_INFO(_node->get_logger(), "Avoid x: %f, y: %f, z: %f", bbox.pos_x, bbox.pos_y, bbox.pos_z); + // RCLCPP_INFO(_node->get_logger(), "Current Avoid x: %f, y: %f, z: %f", current_avoid_target_bbox.pos_x, current_avoid_target_bbox.pos_y, current_avoid_target_bbox.pos_z); + + if (bbox.name == target_bbox_name) { + current_target_bbox = bbox; + found_target = true; + target_disappeared_counter = 0; + } + } + + if (!found_target) { + target_disappeared_counter++; + } +}; + +bool BboxBottomCenteringTwistActionServer::isTwistDone(const std::shared_ptr goal) { + return isDepthDone(goal->depth) && isRollDone(goal->roll) && isPitchDone(goal->pitch); +}; + +bool BboxBottomCenteringTwistActionServer::isCenteringTwistDone() { + return abs(current_target_bbox.horizontal_angle) < target_threshold_x && abs(current_target_bbox.vertical_angle) < target_threshold_y; +}; + +bool BboxBottomCenteringTwistActionServer::isTargetLost() { + return target_disappeared_counter > target_lost_thresh; +}; + +void BboxBottomCenteringTwistActionServer::execute(const std::shared_ptr> goal_handle) { + + auto twistSrvRequest = std::make_shared(); + + RCLCPP_INFO(_node->get_logger(), "Execute action"); + while (!twistSrvClient->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(_node->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_ERROR(_node->get_logger(), "Service %s not available!", _node->get_parameter("set_twist_srv").as_string().c_str()); + return; + } + + // get goal data + const auto goal = goal_handle->get_goal(); + auto goal_result = std::make_shared(); + goal_result->success = false; + + // check duration + if (goal->duration < 0.0) { + goal_result->success = false; + goal_handle->abort(goal_result); + RCLCPP_ERROR(_node->get_logger(), "Duration value must be greater than 0.0"); + return; + } + + bboxArraySub = _node->create_subscription( + goal->bbox_topic, 10, + std::bind(&BboxBottomCenteringTwistActionServer::bboxArrayCallback, this, std::placeholders::_1)); + + // send service request + target_bbox_name = goal->bbox_name; + //target_distance_threshold = goal->distance_threshold; + target_lost_thresh = goal->lost_threshold; + twistSrvRequest->surge = goal->surge; + twistSrvRequest->sway = goal->sway; + twistSrvRequest->depth = goal->depth; + twistSrvRequest->roll = goal->roll; + target_threshold_x = goal->threshold_x; + target_threshold_y = goal->threshold_y; + float e = 2.71828; + + rclcpp::Rate checkRate(goal->centering_rate); + AsyncTimer timer(goal->duration * 1000); + timer.start(); + + while (rclcpp::ok()) { + if (isTargetLost()) { + RCLCPP_ERROR(_node->get_logger(), "Target lost!"); + goal_result->success = false; + RCLCPP_INFO(_node->get_logger(), "Goal canceled"); + + target_disappeared_counter = 0; + current_target_bbox.pos_x = 1000.0; + current_target_bbox.pos_y = 1000.0; + current_target_bbox.pos_z = 1000.0; + current_target_bbox.horizontal_angle = 1000.0; + current_target_bbox.vertical_angle = 1000.0; + current_avoid_target_bbox.pos_x = 1000.0; + current_avoid_target_bbox.pos_y = 1000.0; + current_avoid_target_bbox.pos_z = 1000.0; + target_bbox_name = ""; + bboxArraySub.reset(); + + // stop maneuvr service request + stopTwist(twistSrvRequest); + goal_handle->succeed(goal_result); + return; + } + + if (!timer.isBusy() && isTargetLost()) { + RCLCPP_ERROR(_node->get_logger(), "Twist done by duration %f, target lost!", goal->duration); + goal_result->success = false; + RCLCPP_INFO(_node->get_logger(), "Goal canceled"); + + target_disappeared_counter = 0; + current_target_bbox.pos_x = 1000.0; + current_target_bbox.pos_y = 1000.0; + current_target_bbox.pos_z = 1000.0; + current_target_bbox.horizontal_angle = 1000.0; + current_target_bbox.vertical_angle = 1000.0; + current_avoid_target_bbox.pos_x = 1000.0; + current_avoid_target_bbox.pos_y = 1000.0; + current_avoid_target_bbox.pos_z = 1000.0; + target_bbox_name = ""; + bboxArraySub.reset(); + + // stop maneuvr service request + stopTwist(twistSrvRequest); + goal_handle->succeed(goal_result); + return; + } + + if (isCenteringTwistDone()) { + RCLCPP_INFO(_node->get_logger(), "Twist done OK"); + break; + } + + twistSrvRequest->sway = 0; + + float distance_surge = 8; + float new_speed_surge = fmin((pow(e, abs(current_target_bbox.vertical_angle)) / pow(e, distance_surge)) * abs(goal->surge), abs(goal->surge)); + if (current_target_bbox.vertical_angle < -target_threshold_y) { + twistSrvRequest->surge = new_speed_surge; + } + else if (current_target_bbox.vertical_angle > target_threshold_y) { + twistSrvRequest->surge = -new_speed_surge; + } + + float distance_sway = 8; + float new_speed_sway = fmin((pow(e, abs(current_target_bbox.horizontal_angle)) / pow(e, distance_sway)) * abs(goal->sway), abs(goal->sway)); + if (current_target_bbox.horizontal_angle < -target_threshold_x) { + twistSrvRequest->sway = -new_speed_sway; + } + else if (current_target_bbox.horizontal_angle > target_threshold_x) { + twistSrvRequest->sway = new_speed_sway; + } + + + RCLCPP_INFO(_node->get_logger(), "Current (x, y) to center: (%f, %f)", current_target_bbox.vertical_angle, current_target_bbox.horizontal_angle); + RCLCPP_INFO(_node->get_logger(), "Current (sway, surge): (%f, %f)", twistSrvRequest->sway, twistSrvRequest->surge); + // check if service success + twistSrvClient->async_send_request(twistSrvRequest).wait(); + + 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; + current_target_bbox.pos_x = 1000.0; + current_target_bbox.pos_y = 1000.0; + current_target_bbox.pos_z = 1000.0; + current_target_bbox.horizontal_angle = 0.0; + current_avoid_target_bbox.pos_x = 1000.0; + current_avoid_target_bbox.pos_y = 1000.0; + current_avoid_target_bbox.pos_z = 1000.0; + target_bbox_name = ""; + bboxArraySub.reset(); + + // stop maneuvr service request + stopTwist(twistSrvRequest); + return; + } + // rclcpp::spin_some(_node); + checkRate.sleep(); + } + target_disappeared_counter = 0; + current_target_bbox.pos_x = 1000.0; + current_target_bbox.pos_y = 1000.0; + current_target_bbox.pos_z = 1000.0; + current_target_bbox.horizontal_angle = 0.0; + current_avoid_target_bbox.pos_x = 1000.0; + current_avoid_target_bbox.pos_y = 1000.0; + current_avoid_target_bbox.pos_z = 1000.0; + target_bbox_name = ""; + bboxArraySub.reset(); + + RCLCPP_INFO(_node->get_logger(), "Done moving"); + + // stop maneuvr service request + stopTwist(twistSrvRequest); + + if (rclcpp::ok()) { + goal_result->success = true; + RCLCPP_INFO(_node->get_logger(), "Goal succeeded"); + goal_handle->succeed(goal_result); + } + +}; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + std::shared_ptr node = rclcpp::Node::make_shared("bbox_bottom_centering_twist_action_server"); + node->declare_parameter("bbox_bottom_centering_twist_action", "/stingray/actions/bbox_bottom_centering_twist"); + BboxBottomCenteringTwistActionServer server = BboxBottomCenteringTwistActionServer(node, node->get_parameter("bbox_bottom_centering_twist_action").as_string()); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +}; \ No newline at end of file diff --git a/src/stingray_movement/src/BboxCenteringTwistActionServer.cpp b/src/stingray_movement/src/BboxCenteringTwistActionServer.cpp index ee8d69a..5daef02 100644 --- a/src/stingray_movement/src/BboxCenteringTwistActionServer.cpp +++ b/src/stingray_movement/src/BboxCenteringTwistActionServer.cpp @@ -10,7 +10,6 @@ BboxCenteringTwistActionServer::BboxCenteringTwistActionServer(std::shared_ptrget_logger(), "Current Avoid x: %f, y: %f, z: %f", current_avoid_target_bbox.pos_x, current_avoid_target_bbox.pos_y, current_avoid_target_bbox.pos_z); if (std::find(target_avoid_bbox_name_array.begin(), target_avoid_bbox_name_array.end(), bbox.name) != target_avoid_bbox_name_array.end()) { - RCLCPP_INFO(_node->get_logger(), "Found %s: %f, y: %f, z: %f", bbox.name.c_str(), bbox.pos_x, bbox.pos_y, bbox.pos_z); + // RCLCPP_INFO(_node->get_logger(), "Found %s: %f, y: %f, z: %f", bbox.name.c_str(), bbox.pos_x, bbox.pos_y, bbox.pos_z); if (bbox.pos_z < current_avoid_target_bbox.pos_z) { current_avoid_target_bbox = bbox; } @@ -102,7 +101,7 @@ void BboxCenteringTwistActionServer::execute(const std::shared_ptrpitch = goal->pitch; float sway_sum = 0.0; // закостылил, ибо не знал какой брать - int parrot_rate = 5; + int parrot_rate = 30; rclcpp::Rate checkRate(goal->centering_rate); AsyncTimer timer(goal->duration * 1000);