From 1c09d65334c06219ee3468e41be91ef90e5cfe83 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 20 Dec 2024 12:50:51 +0000 Subject: [PATCH] Removed dock plugins Signed-off-by: Jakub Delicat --- husarion_ugv_manager/CMakeLists.txt | 20 +- .../plugins/action/dock_robot_node.hpp | 81 ------ .../plugins/action/undock_robot_node.hpp | 66 ----- husarion_ugv_manager/package.xml | 1 - .../src/plugins/action/dock_robot_node.cpp | 89 ------- .../src/plugins/action/undock_robot_node.cpp | 66 ----- .../plugins/action/test_dock_robot_node.cpp | 245 ------------------ .../plugins/action/test_undock_robot_node.cpp | 173 ------------- 8 files changed, 1 insertion(+), 740 deletions(-) delete mode 100644 husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/dock_robot_node.hpp delete mode 100644 husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/undock_robot_node.hpp delete mode 100644 husarion_ugv_manager/src/plugins/action/dock_robot_node.cpp delete mode 100644 husarion_ugv_manager/src/plugins/action/undock_robot_node.cpp delete mode 100644 husarion_ugv_manager/test/plugins/action/test_dock_robot_node.cpp delete mode 100644 husarion_ugv_manager/test/plugins/action/test_undock_robot_node.cpp diff --git a/husarion_ugv_manager/CMakeLists.txt b/husarion_ugv_manager/CMakeLists.txt index 68526ae63..b17119959 100644 --- a/husarion_ugv_manager/CMakeLists.txt +++ b/husarion_ugv_manager/CMakeLists.txt @@ -56,13 +56,6 @@ add_library(shutdown_hosts_from_file_bt_node SHARED target_link_libraries(shutdown_hosts_from_file_bt_node ssh yaml-cpp) list(APPEND plugin_libs shutdown_hosts_from_file_bt_node) -add_library(dock_robot_bt_node SHARED src/plugins/action/dock_robot_node.cpp) -list(APPEND plugin_libs dock_robot_bt_node) - -add_library(undock_robot_bt_node SHARED - src/plugins/action/undock_robot_node.cpp) -list(APPEND plugin_libs undock_robot_bt_node) - add_library(check_bool_msg_bt_node SHARED src/plugins/condition/check_bool_msg.cpp) list(APPEND plugin_libs check_bool_msg_bt_node) @@ -125,6 +118,7 @@ install(DIRECTORY behavior_trees config launch DESTINATION share/${PROJECT_NAME}) install(DIRECTORY include/ DESTINATION include/) +install(DIRECTORY test/utils DESTINATION include/test/utils) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) @@ -162,18 +156,6 @@ if(BUILD_TESTING) src/plugins/action/shutdown_hosts_from_file_node.cpp) list(APPEND plugin_tests ${PROJECT_NAME}_test_shutdown_hosts_from_file_node) - ament_add_gtest( - ${PROJECT_NAME}_test_dock_robot_node - test/plugins/action/test_dock_robot_node.cpp - src/plugins/action/dock_robot_node.cpp) - list(APPEND plugin_tests ${PROJECT_NAME}_test_dock_robot_node) - - ament_add_gtest( - ${PROJECT_NAME}_test_undock_robot_node - test/plugins/action/test_undock_robot_node.cpp - src/plugins/action/undock_robot_node.cpp) - list(APPEND plugin_tests ${PROJECT_NAME}_test_undock_robot_node) - ament_add_gtest( ${PROJECT_NAME}_test_check_bool_msg test/plugins/condition/test_check_bool_msg.cpp diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/dock_robot_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/dock_robot_node.hpp deleted file mode 100644 index a050b947e..000000000 --- a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/dock_robot_node.hpp +++ /dev/null @@ -1,81 +0,0 @@ -// Copyright 2024 Husarion sp. z o.o. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_ -#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_ - -#include -#include -#include - -#include - -#include -#include - -namespace husarion_ugv_manager -{ - -class DockRobot : public BT::RosActionNode -{ - using DockRobotAction = opennav_docking_msgs::action::DockRobot; - using DockRobotActionResult = DockRobotAction::Result; - -public: - DockRobot(const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) - : RosActionNode(name, conf, params) - { - } - - bool setGoal(Goal & goal) override; - - void onHalt() override; - - BT::NodeStatus onResultReceived(const WrappedResult & wr) override; - - virtual BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override; - - static BT::PortsList providedPorts() - { - return providedBasicPorts( - {BT::InputPort( - "use_dock_id", true, "Determines whether to use the dock's ID or dock pose fields."), - BT::InputPort( - "dock_id", - "Specifies the dock's ID or name from the dock database (used if 'use_dock_id' is true)."), - BT::InputPort( - "dock_pose", - "Specifies the dock's pose (used if 'use_dock_id' is false). Format: " - "\"x;y;z;roll;pitch;yaw;frame_id\""), - BT::InputPort( - "dock_type", - "Defines the type of dock being used when docking via pose. Not needed if only one dock " - "type is available."), - BT::InputPort( - "max_staging_time", 120.0, - "Maximum time allowed (in seconds) for navigating to the dock's staging pose."), - BT::InputPort( - "navigate_to_staging_pose", true, - "Specifies whether the robot should autonomously navigate to the staging pose."), - - BT::OutputPort( - "error_code", "Returns an error code indicating the reason for failure, if any."), - BT::OutputPort( - "num_retries", "Reports the number of retry attempts made during the docking process.")}); - } -}; - -} // namespace husarion_ugv_manager - -#endif // HUSARION_UGV_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_ diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/undock_robot_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/undock_robot_node.hpp deleted file mode 100644 index e72160498..000000000 --- a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/undock_robot_node.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2024 Husarion sp. z o.o. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_ -#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_ - -#include -#include -#include - -#include - -#include -#include - -namespace husarion_ugv_manager -{ - -class UndockRobot : public BT::RosActionNode -{ - using UndockRobotAction = opennav_docking_msgs::action::UndockRobot; - using UndockRobotActionResult = UndockRobotAction::Result; - -public: - UndockRobot( - const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) - : RosActionNode(name, conf, params) - { - } - - bool setGoal(Goal & goal) override; - - void onHalt() override; - - BT::NodeStatus onResultReceived(const WrappedResult & wr) override; - - virtual BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override; - - static BT::PortsList providedPorts() - { - return providedBasicPorts( - {BT::InputPort( - "dock_type", "Specifies the dock plugin type to use for undocking."), - BT::InputPort( - "max_undocking_time", 30.0, - "Maximum allowable time (in seconds) to undock and return to the staging pose."), - - BT::OutputPort( - "error_code", "Returns an error code if the undocking process fails.")}); - } -}; - -} // namespace husarion_ugv_manager - -#endif // HUSARION_UGV_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_ diff --git a/husarion_ugv_manager/package.xml b/husarion_ugv_manager/package.xml index 4129bb1ab..46445054f 100644 --- a/husarion_ugv_manager/package.xml +++ b/husarion_ugv_manager/package.xml @@ -26,7 +26,6 @@ iputils-ping libboost-dev libssh-dev - opennav_docking_msgs rclcpp rclcpp_action sensor_msgs diff --git a/husarion_ugv_manager/src/plugins/action/dock_robot_node.cpp b/husarion_ugv_manager/src/plugins/action/dock_robot_node.cpp deleted file mode 100644 index b7e3dd3c5..000000000 --- a/husarion_ugv_manager/src/plugins/action/dock_robot_node.cpp +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright 2024 Husarion sp. z o.o. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "husarion_ugv_manager/plugins/action/dock_robot_node.hpp" -#include "husarion_ugv_manager/behavior_tree_utils.hpp" - -namespace husarion_ugv_manager -{ - -bool DockRobot::setGoal(Goal & goal) -{ - if (!this->getInput("dock_type", goal.dock_type) || goal.dock_type.empty()) { - RCLCPP_ERROR_STREAM( - this->logger(), GetLoggerPrefix(name()) << "Failed to get input [dock_type]"); - return false; - } - - if (!this->getInput("use_dock_id", goal.use_dock_id)) { - RCLCPP_WARN_STREAM( - this->logger(), GetLoggerPrefix(name()) - << "use_dock_id not set, using default value: " << goal.use_dock_id); - } - - if ( - (!this->getInput("dock_id", goal.dock_id) || goal.dock_id.empty()) && - goal.use_dock_id) { - RCLCPP_ERROR_STREAM(this->logger(), GetLoggerPrefix(name()) << "Failed to get input [dock_id]"); - return false; - } - - if (!this->getInput("navigate_to_staging_pose", goal.navigate_to_staging_pose)) { - RCLCPP_WARN_STREAM( - this->logger(), GetLoggerPrefix(name()) << "navigate_to_staging_pose not set, using default " - "value: " - << goal.navigate_to_staging_pose); - } - - if ( - !this->getInput("max_staging_time", goal.max_staging_time) && - goal.navigate_to_staging_pose) { - RCLCPP_ERROR_STREAM( - this->logger(), GetLoggerPrefix(name()) << "Failed to get input [max_staging_time]"); - return false; - } - - return true; -} - -BT::NodeStatus DockRobot::onResultReceived(const WrappedResult & wr) -{ - const auto & result = wr.result; - - this->setOutput("error_code", result->error_code); - this->setOutput("num_retries", result->num_retries); - - if (result->success) { - return BT::NodeStatus::SUCCESS; - } - - return BT::NodeStatus::FAILURE; -} - -BT::NodeStatus DockRobot::onFailure(BT::ActionNodeErrorCode error) -{ - RCLCPP_ERROR_STREAM( - this->logger(), GetLoggerPrefix(name()) << ": onFailure with error: " << toStr(error)); - return BT::NodeStatus::FAILURE; -} - -void DockRobot::onHalt() -{ - RCLCPP_INFO_STREAM(this->logger(), GetLoggerPrefix(name()) << ": onHalt"); -} - -} // namespace husarion_ugv_manager - -#include "behaviortree_ros2/plugins.hpp" -CreateRosNodePlugin(husarion_ugv_manager::DockRobot, "DockRobot"); diff --git a/husarion_ugv_manager/src/plugins/action/undock_robot_node.cpp b/husarion_ugv_manager/src/plugins/action/undock_robot_node.cpp deleted file mode 100644 index bee50f39b..000000000 --- a/husarion_ugv_manager/src/plugins/action/undock_robot_node.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2024 Husarion sp. z o.o. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "husarion_ugv_manager/plugins/action/undock_robot_node.hpp" -#include "husarion_ugv_manager/behavior_tree_utils.hpp" - -namespace husarion_ugv_manager -{ - -bool UndockRobot::setGoal(Goal & goal) -{ - if (!this->getInput("dock_type", goal.dock_type) || goal.dock_type.empty()) { - RCLCPP_ERROR_STREAM( - this->logger(), GetLoggerPrefix(name()) << "Failed to get input [dock_type]"); - return false; - } - - if (!this->getInput("max_undocking_time", goal.max_undocking_time)) { - RCLCPP_ERROR_STREAM( - this->logger(), GetLoggerPrefix(name()) << "Failed to get input [max_undocking_time]"); - return false; - } - - return true; -} - -BT::NodeStatus UndockRobot::onResultReceived(const WrappedResult & wr) -{ - const auto & result = wr.result; - - this->setOutput("error_code", result->error_code); - - if (result->success) { - return BT::NodeStatus::SUCCESS; - } - - return BT::NodeStatus::FAILURE; -} - -BT::NodeStatus UndockRobot::onFailure(BT::ActionNodeErrorCode error) -{ - RCLCPP_ERROR_STREAM( - this->logger(), GetLoggerPrefix(name()) << ": onFailure with error: " << toStr(error)); - return BT::NodeStatus::FAILURE; -} - -void UndockRobot::onHalt() -{ - RCLCPP_INFO_STREAM(this->logger(), GetLoggerPrefix(name()) << ": onHalt"); -} - -} // namespace husarion_ugv_manager - -#include "behaviortree_ros2/plugins.hpp" -CreateRosNodePlugin(husarion_ugv_manager::UndockRobot, "UndockRobot"); diff --git a/husarion_ugv_manager/test/plugins/action/test_dock_robot_node.cpp b/husarion_ugv_manager/test/plugins/action/test_dock_robot_node.cpp deleted file mode 100644 index 97f66cecc..000000000 --- a/husarion_ugv_manager/test/plugins/action/test_dock_robot_node.cpp +++ /dev/null @@ -1,245 +0,0 @@ -// Copyright 2024 Husarion sp. z o.o. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include - -#include -#include -#include - -#include - -#include "husarion_ugv_manager/plugins/action/dock_robot_node.hpp" -#include "utils/plugin_test_utils.hpp" - -class TestDockRobot : public husarion_ugv_manager::plugin_test_utils::PluginTestUtils -{ -public: - using Action = opennav_docking_msgs::action::DockRobot; - using ActionResult = opennav_docking_msgs::action::DockRobot::Result; - using GoalHandle = rclcpp_action::ServerGoalHandle; - using GoalResponse = rclcpp_action::GoalResponse; - using CancelResponse = rclcpp_action::CancelResponse; - - void CreateActionServer( - GoalResponse goal_response, CancelResponse cancel_response, bool success, - std::uint16_t error_code) - { - auto handle_goal = - [&, goal_response]( - const rclcpp_action::GoalUUID & /*uuid*/, - std::shared_ptr /*goal*/) -> rclcpp_action::GoalResponse { - return goal_response; - }; - - auto handle_cancel = [&, cancel_response](const std::shared_ptr /*goal_handle*/) - -> rclcpp_action::CancelResponse { return cancel_response; }; - - auto handle_accepted = [&, success, - error_code](const std::shared_ptr goal_handle) -> void { - ActionResult::SharedPtr result = std::make_shared(); - result->success = success; - result->error_code = error_code; - result->num_retries = 0; - goal_handle->succeed(result); - }; - - CreateAction("test_dock_action", handle_goal, handle_cancel, handle_accepted); - } -}; - -TEST_F(TestDockRobot, GoodLoadingDockRobotPlugin) -{ - std::map params = { - {"action_name", "test_dock_action"}, - {"use_dock_id", "true"}, - {"dock_id", "test_dock"}, - {"dock_type", "test_dock_type"}, - {"max_staging_time", "5.0"}, - {"navigate_to_staging_pose", "false"}, - }; - - RegisterNodeWithParams("DockRobot"); - - ASSERT_NO_THROW({ CreateTree("DockRobot", params); }); -} - -TEST_F(TestDockRobot, WrongLoadingDockRobotPlugin) -{ - std::map params = { - {"action_name", ""}, {"use_dock_id", "true"}, - {"dock_id", "test_dock"}, {"dock_type", "test_dock_type"}, - {"max_staging_time", "5.0"}, {"navigate_to_staging_pose", "false"}, - }; - - RegisterNodeWithParams("DockRobot"); - - EXPECT_THROW({ CreateTree("WrongDockRobot", params); }, BT::RuntimeError); -} - -TEST_F(TestDockRobot, WrongCallDockRobotServerNotInitialized) -{ - std::map params = { - {"action_name", "test_dock_action"}, - {"use_dock_id", "true"}, - {"dock_id", "test_dock"}, - {"dock_type", "test_dock_type"}, - {"max_staging_time", "5.0"}, - {"navigate_to_staging_pose", "false"}, - }; - RegisterNodeWithParams("DockRobot"); - - CreateTree("DockRobot", params); - - auto & tree = GetTree(); - - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::FAILURE); -} - -TEST_F(TestDockRobot, WrongCallDockRobotServerWithNoDockID) -{ - CreateActionServer( - GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); - - std::map params = { - {"action_name", "test_dock_action"}, {"use_dock_id", "true"}, - {"dock_type", "test_dock_type"}, {"max_staging_time", "5.0"}, - {"navigate_to_staging_pose", "false"}, - }; - - RegisterNodeWithParams("DockRobot"); - CreateTree("DockRobot", params); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::FAILURE); -} - -TEST_F(TestDockRobot, CallDockRobotServerWithoutDockID) -{ - CreateActionServer( - GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); - - std::map params = { - {"action_name", "test_dock_action"}, {"use_dock_id", "false"}, - {"dock_type", "test_dock_type"}, {"max_staging_time", "5.0"}, - {"navigate_to_staging_pose", "false"}, - }; - - RegisterNodeWithParams("DockRobot"); - CreateTree("DockRobot", params); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::SUCCESS); -} - -TEST_F(TestDockRobot, CallDockRobotServerWithEmptyDockID) -{ - CreateActionServer( - GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); - - std::map params = { - {"action_name", "test_dock_action"}, - {"use_dock_id", "true"}, - {"dock_id", ""}, - {"dock_type", "test_dock_type"}, - {"max_staging_time", "5.0"}, - {"navigate_to_staging_pose", "true"}, - }; - - RegisterNodeWithParams("DockRobot"); - CreateTree("DockRobot", params); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::FAILURE); -} - -TEST_F(TestDockRobot, CallDockRobotServerWithEmptyDockType) -{ - CreateActionServer( - GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); - - std::map params = { - {"action_name", "test_dock_action"}, - {"use_dock_id", "true"}, - {"dock_id", "main_dock"}, - {"dock_type", ""}, - {"max_staging_time", "5.0"}, - {"navigate_to_staging_pose", "true"}, - }; - - RegisterNodeWithParams("DockRobot"); - CreateTree("DockRobot", params); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::FAILURE); -} - -TEST_F(TestDockRobot, CallDockRobotServerWithNavigateToStagingPoseFailure) -{ - CreateActionServer(GoalResponse::REJECT, CancelResponse::ACCEPT, true, ActionResult::NONE); - - std::map params = { - {"action_name", "test_dock_action"}, - {"use_dock_id", "true"}, - {"dock_id", "main_dock"}, - {"dock_type", "test_dock_type"}, - {"max_staging_time", "5.0"}, - {"navigate_to_staging_pose", "true"}, - }; - - RegisterNodeWithParams("DockRobot"); - CreateTree("DockRobot", params); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::FAILURE); -} - -TEST_F(TestDockRobot, CallDockRobotServerWithNavigateToStagingPoseSuccess) -{ - CreateActionServer( - GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); - - std::map params = { - {"action_name", "test_dock_action"}, - {"use_dock_id", "true"}, - {"dock_id", "main_dock"}, - {"dock_type", "test_dock_type"}, - {"max_staging_time", "5.0"}, - {"navigate_to_staging_pose", "true"}, - }; - - RegisterNodeWithParams("DockRobot"); - CreateTree("DockRobot", params); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::SUCCESS); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - - auto result = RUN_ALL_TESTS(); - - return result; -} diff --git a/husarion_ugv_manager/test/plugins/action/test_undock_robot_node.cpp b/husarion_ugv_manager/test/plugins/action/test_undock_robot_node.cpp deleted file mode 100644 index ea8f53692..000000000 --- a/husarion_ugv_manager/test/plugins/action/test_undock_robot_node.cpp +++ /dev/null @@ -1,173 +0,0 @@ -// Copyright 2024 Husarion sp. z o.o. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include - -#include -#include -#include - -#include - -#include "husarion_ugv_manager/plugins/action/undock_robot_node.hpp" -#include "utils/plugin_test_utils.hpp" - -class TestUndockRobot : public husarion_ugv_manager::plugin_test_utils::PluginTestUtils -{ -public: - using Action = opennav_docking_msgs::action::UndockRobot; - using ActionResult = opennav_docking_msgs::action::UndockRobot::Result; - using GoalHandleAction = rclcpp_action::ServerGoalHandle; - using GoalResponse = rclcpp_action::GoalResponse; - using CancelResponse = rclcpp_action::CancelResponse; - - void CreateActionServer( - GoalResponse goal_response, CancelResponse cancel_response, bool success, - std::uint16_t error_code) - { - auto handle_goal = - [&, goal_response]( - const rclcpp_action::GoalUUID & /*uuid*/, - std::shared_ptr /*goal*/) -> rclcpp_action::GoalResponse { - return goal_response; - }; - - auto handle_cancel = - [&, cancel_response]( - const std::shared_ptr /*goal_handle*/) -> rclcpp_action::CancelResponse { - return cancel_response; - }; - - auto handle_accepted = - [&, success, error_code](const std::shared_ptr goal_handle) -> void { - ActionResult::SharedPtr result = std::make_shared(); - result->success = success; - result->error_code = error_code; - goal_handle->succeed(result); - }; - - CreateAction("test_undock_action", handle_goal, handle_cancel, handle_accepted); - } -}; - -TEST_F(TestUndockRobot, GoodLoadingUndockRobotPlugin) -{ - std::map params = { - {"action_name", "test_undock_action"}, - {"dock_type", "test_dock_type"}, - {"max_undocking_time", "5.0"}, - }; - - RegisterNodeWithParams("UndockRobot"); - - ASSERT_NO_THROW({ CreateTree("UndockRobot", params); }); -} - -TEST_F(TestUndockRobot, WrongLoadingUndockRobotPlugin) -{ - std::map params = { - {"action_name", ""}, - {"dock_type", "test_dock_type"}, - {"max_undocking_time", "5.0"}, - }; - - RegisterNodeWithParams("UndockRobot"); - - EXPECT_THROW({ CreateTree("WrongUndockRobot", params); }, BT::RuntimeError); -} - -TEST_F(TestUndockRobot, WrongCallUndockRobotServerNotInitialized) -{ - std::map params = { - {"action_name", "test_undock_action"}, - {"dock_type", "test_dock_type"}, - {"max_undocking_time", "5.0"}, - }; - - RegisterNodeWithParams("UndockRobot"); - - CreateTree("UndockRobot", params); - - auto & tree = GetTree(); - - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::FAILURE); -} - -TEST_F(TestUndockRobot, WrongCallUndockRobotServerWithNoDockType) -{ - CreateActionServer( - GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); - - std::map params = { - {"action_name", "test_undock_action"}, - {"dock_type", ""}, - {"max_undocking_time", "5.0"}, - }; - - RegisterNodeWithParams("UndockRobot"); - CreateTree("UndockRobot", params); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(1000)); - EXPECT_EQ(status, BT::NodeStatus::FAILURE); -} - -TEST_F(TestUndockRobot, CallUndockRobotServerFailure) -{ - CreateActionServer(GoalResponse::REJECT, CancelResponse::ACCEPT, true, ActionResult::NONE); - - std::map params = { - {"action_name", "test_undock_action"}, - {"dock_type", "test_dock_type"}, - {"max_undocking_time", "5.0"}, - }; - - RegisterNodeWithParams("UndockRobot"); - CreateTree("UndockRobot", params); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(1000)); - EXPECT_EQ(status, BT::NodeStatus::FAILURE); -} - -TEST_F(TestUndockRobot, CallUndockRobotServerSuccess) -{ - CreateActionServer( - GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); - - std::map params = { - {"action_name", "test_undock_action"}, - {"dock_type", "test_dock_type"}, - {"max_undocking_time", "5.0"}, - }; - - RegisterNodeWithParams("UndockRobot"); - CreateTree("UndockRobot", params); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(1000)); - EXPECT_EQ(status, BT::NodeStatus::SUCCESS); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - - auto result = RUN_ALL_TESTS(); - - return result; -}