diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index d69bbd31..1f7bcfb9 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -59,6 +59,10 @@ add_library(undock_robot_bt_node SHARED src/plugins/action/undock_robot_node.cpp) list(APPEND plugin_libs undock_robot_bt_node) +add_library(are_buttons_pressed_bt_node SHARED + src/plugins/condition/are_buttons_pressed.cpp) +list(APPEND plugin_libs are_buttons_pressed_bt_node) + add_library(tick_after_timeout_bt_node SHARED src/plugins/decorator/tick_after_timeout_node.cpp) list(APPEND plugin_libs tick_after_timeout_bt_node) @@ -154,9 +158,14 @@ if(BUILD_TESTING) ${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_are_buttons_pressed + test/plugins/condition/test_are_buttons_pressed.cpp + src/plugins/condition/are_buttons_pressed.cpp) + list(APPEND plugin_tests ${PROJECT_NAME}_test_are_buttons_pressed) + ament_add_gtest( ${PROJECT_NAME}_test_tick_after_timeout_node test/plugins/decorator/test_tick_after_timeout_node.cpp diff --git a/panther_manager/include/panther_manager/behavior_tree_utils.hpp b/panther_manager/include/panther_manager/behavior_tree_utils.hpp index 360e174e..5ee2331e 100644 --- a/panther_manager/include/panther_manager/behavior_tree_utils.hpp +++ b/panther_manager/include/panther_manager/behavior_tree_utils.hpp @@ -90,4 +90,31 @@ inline std::string GetLoggerPrefix(const std::string & bt_node_name) } } // namespace panther_manager +namespace BT +{ +/** + * @brief Converts a string to a vector of integers. + * + * @param str The string to convert. + * @return std::vector The vector of integers. + * + * @throw BT::RuntimeError Throws when there is no input or cannot parse int. + */ +template <> +inline std::vector convertFromString>(StringView str) +{ + auto parts = BT::splitString(str, ';'); + if (!parts.size()) { + throw BT::RuntimeError("invalid input"); + } else { + std::vector result; + for (auto & part : parts) { + result.push_back(convertFromString(part)); + } + + return result; + } +} +} // namespace BT + #endif // PANTHER_MANAGER_BEHAVIOR_TREE_UTILS_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/condition/are_buttons_pressed.hpp b/panther_manager/include/panther_manager/plugins/condition/are_buttons_pressed.hpp new file mode 100644 index 00000000..8b0534b5 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/condition/are_buttons_pressed.hpp @@ -0,0 +1,55 @@ +// 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 PANTHER_MANAGER_PLUGINS_CONDITION_ARE_BUTTONS_PRESSED_HPP_ +#define PANTHER_MANAGER_PLUGINS_CONDITION_ARE_BUTTONS_PRESSED_HPP_ + +#include +#include +#include + +#include +#include + +#include + +#include "panther_manager/behavior_tree_utils.hpp" + +namespace panther_manager +{ + +class AreButtonsPressed : public BT::RosTopicSubNode +{ +public: + AreButtonsPressed( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) + : BT::RosTopicSubNode(name, conf, params) + { + } + + BT::NodeStatus onTick(const std::shared_ptr & last_msg); + + static BT::PortsList providedPorts() + { + return providedBasicPorts( + {BT::InputPort>("buttons", "state of buttons to accept a condition")}); + } + +private: + std::vector buttons_; +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_PLUGINS_CONDITION_ARE_BUTTONS_PRESSED_HPP_ diff --git a/panther_manager/package.xml b/panther_manager/package.xml index cf2a8812..6da23d97 100644 --- a/panther_manager/package.xml +++ b/panther_manager/package.xml @@ -26,6 +26,7 @@ panther_utils rclcpp rclcpp_action + sensor_msgs std_srvs yaml-cpp diff --git a/panther_manager/src/plugins/condition/are_buttons_pressed.cpp b/panther_manager/src/plugins/condition/are_buttons_pressed.cpp new file mode 100644 index 00000000..1aad81af --- /dev/null +++ b/panther_manager/src/plugins/condition/are_buttons_pressed.cpp @@ -0,0 +1,48 @@ +// 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 "panther_manager/plugins/condition/are_buttons_pressed.hpp" + +#include "panther_manager/behavior_tree_utils.hpp" + +namespace panther_manager +{ + +BT::NodeStatus AreButtonsPressed::onTick(const std::shared_ptr & last_msg) +{ + getInput>("buttons", buttons_); + + if (!last_msg) { + RCLCPP_WARN_STREAM(this->logger(), GetLoggerPrefix(name()) << "There is no joy messages!"); + return BT::NodeStatus::FAILURE; + } + + if (last_msg->buttons.size() < buttons_.size()) { + RCLCPP_ERROR_STREAM( + this->logger(), GetLoggerPrefix(name()) << "Joy message has " << last_msg->buttons.size() + << " buttons, expected at least " << buttons_.size()); + return BT::NodeStatus::FAILURE; + } + + if (std::equal(buttons_.begin(), buttons_.end(), last_msg->buttons.begin())) { + return BT::NodeStatus::SUCCESS; + } + + return BT::NodeStatus::FAILURE; +} + +} // namespace panther_manager + +#include "behaviortree_ros2/plugins.hpp" +CreateRosNodePlugin(panther_manager::AreButtonsPressed, "AreButtonsPressed"); diff --git a/panther_manager/test/plugins/action/test_call_set_bool_service_node.cpp b/panther_manager/test/plugins/action/test_call_set_bool_service_node.cpp index c9526698..37142c9a 100644 --- a/panther_manager/test/plugins/action/test_call_set_bool_service_node.cpp +++ b/panther_manager/test/plugins/action/test_call_set_bool_service_node.cpp @@ -139,5 +139,7 @@ int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + auto result = RUN_ALL_TESTS(); + + return result; } diff --git a/panther_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp b/panther_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp index d44ff4ec..45038a06 100644 --- a/panther_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp +++ b/panther_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp @@ -175,5 +175,7 @@ int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + auto result = RUN_ALL_TESTS(); + + return result; } diff --git a/panther_manager/test/plugins/action/test_call_trigger_service_node.cpp b/panther_manager/test/plugins/action/test_call_trigger_service_node.cpp index e0aa3050..d99e1027 100644 --- a/panther_manager/test/plugins/action/test_call_trigger_service_node.cpp +++ b/panther_manager/test/plugins/action/test_call_trigger_service_node.cpp @@ -118,5 +118,7 @@ int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + auto result = RUN_ALL_TESTS(); + + return result; } diff --git a/panther_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp b/panther_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp index edece3f6..2ed57f0c 100644 --- a/panther_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp +++ b/panther_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp @@ -105,5 +105,7 @@ int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + auto result = RUN_ALL_TESTS(); + + return result; } diff --git a/panther_manager/test/plugins/action/test_shutdown_single_host_node.cpp b/panther_manager/test/plugins/action/test_shutdown_single_host_node.cpp index b174b7dc..5e0c5196 100644 --- a/panther_manager/test/plugins/action/test_shutdown_single_host_node.cpp +++ b/panther_manager/test/plugins/action/test_shutdown_single_host_node.cpp @@ -112,5 +112,7 @@ int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + auto result = RUN_ALL_TESTS(); + + return result; } diff --git a/panther_manager/test/plugins/action/test_signal_shutdown_node.cpp b/panther_manager/test/plugins/action/test_signal_shutdown_node.cpp index 9fd109c2..765c84fa 100644 --- a/panther_manager/test/plugins/action/test_signal_shutdown_node.cpp +++ b/panther_manager/test/plugins/action/test_signal_shutdown_node.cpp @@ -65,5 +65,7 @@ int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + auto result = RUN_ALL_TESTS(); + + return result; } diff --git a/panther_manager/test/plugins/condition/test_are_buttons_pressed.cpp b/panther_manager/test/plugins/condition/test_are_buttons_pressed.cpp new file mode 100644 index 00000000..b563e7d5 --- /dev/null +++ b/panther_manager/test/plugins/condition/test_are_buttons_pressed.cpp @@ -0,0 +1,119 @@ +// 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 + +#include "panther_manager/plugins/condition/are_buttons_pressed.hpp" +#include "utils/plugin_test_utils.hpp" + +class TestAreButtonsPressed : public panther_manager::plugin_test_utils::PluginTestUtils +{ +public: + TestAreButtonsPressed(); + void PublishJoyMessage(const std::vector & buttons); + +protected: + rclcpp::Publisher::SharedPtr joy_publisher_; + std::map params_ = {{"topic_name", "joy"}, {"buttons", "0;1;0"}}; +}; + +TestAreButtonsPressed::TestAreButtonsPressed() +{ + RegisterNodeWithParams("AreButtonsPressed"); + joy_publisher_ = bt_node_->create_publisher("joy", 10); +} + +void TestAreButtonsPressed::PublishJoyMessage(const std::vector & buttons) +{ + sensor_msgs::msg::Joy msg; + msg.buttons = buttons; + joy_publisher_->publish(msg); +} + +TEST_F(TestAreButtonsPressed, LoadingAreButtonsPressedPlugin) +{ + ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); +} + +TEST_F(TestAreButtonsPressed, NoMessage) +{ + ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); + + auto & tree = GetTree(); + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + EXPECT_EQ(status, BT::NodeStatus::FAILURE); +} + +TEST_F(TestAreButtonsPressed, WrongMessageTooFewButtons) +{ + ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); + + PublishJoyMessage({0, 1}); + + auto & tree = GetTree(); + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + EXPECT_EQ(status, BT::NodeStatus::FAILURE); +} + +TEST_F(TestAreButtonsPressed, GoodMessageWrongButtonsState) +{ + ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); + + PublishJoyMessage({0, 0, 0}); + + auto & tree = GetTree(); + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + EXPECT_EQ(status, BT::NodeStatus::FAILURE); +} + +TEST_F(TestAreButtonsPressed, GoodMessageWithTooMuchButtonsAndGoodButtonsState) +{ + ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); + + PublishJoyMessage({0, 1, 0, 0, 0, 1}); + + auto & tree = GetTree(); + auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); +} + +TEST_F(TestAreButtonsPressed, GoodMessageGoodButtonsState) +{ + ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); + + PublishJoyMessage({0, 1, 0}); + + 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/panther_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp b/panther_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp index deba7db9..447a5528 100644 --- a/panther_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp +++ b/panther_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp @@ -79,5 +79,7 @@ int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + auto result = RUN_ALL_TESTS(); + + return result; } diff --git a/panther_manager/test/plugins/test_shutdown_host.cpp b/panther_manager/test/plugins/test_shutdown_host.cpp index 25317ee5..22f424ad 100644 --- a/panther_manager/test/plugins/test_shutdown_host.cpp +++ b/panther_manager/test/plugins/test_shutdown_host.cpp @@ -117,5 +117,7 @@ int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + auto result = RUN_ALL_TESTS(); + + return result; } diff --git a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp b/panther_manager/test/plugins/test_shutdown_hosts_node.cpp index 1395f439..1293e24c 100644 --- a/panther_manager/test/plugins/test_shutdown_hosts_node.cpp +++ b/panther_manager/test/plugins/test_shutdown_hosts_node.cpp @@ -155,5 +155,7 @@ int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + auto result = RUN_ALL_TESTS(); + + return result; } diff --git a/panther_manager/test/test_behavior_tree_utils.cpp b/panther_manager/test/test_behavior_tree_utils.cpp index 75b0cb0a..a65c0a07 100644 --- a/panther_manager/test/test_behavior_tree_utils.cpp +++ b/panther_manager/test/test_behavior_tree_utils.cpp @@ -121,6 +121,35 @@ TEST_F(TestRegisterBT, RegisterBehaviorTreeROS) rclcpp::shutdown(); } +TEST(TestConvertFromString, GoodVectorInt) +{ + std::string str = "1;2;3"; + auto result = BT::convertFromString>(str); + + EXPECT_EQ(result.size(), 3); + EXPECT_EQ(result[0], 1); + EXPECT_EQ(result[1], 2); + EXPECT_EQ(result[2], 3); +} + +TEST(TestConvertFromString, EmptyVectorInt) +{ + std::string str = ""; + EXPECT_THROW(BT::convertFromString>(str), BT::RuntimeError); +} + +TEST(TestConvertFromString, InvalidVectorIntWithFloat) +{ + std::string str = "1.0;2;3;4"; + EXPECT_THROW(BT::convertFromString>(str), BT::RuntimeError); +} + +TEST(TestConvertFromString, InvalidVectorIntWithLetter) +{ + std::string str = "a;2;3;4"; + EXPECT_THROW(BT::convertFromString>(str), BT::RuntimeError); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv);