JoySubscription BT node (#397)

* changed main in manager's tests

Signed-off-by: Jakub Delicat <[email protected]>

* Changed to AreButtonsPressed

Signed-off-by: Jakub Delicat <[email protected]>

---------

Signed-off-by: Jakub Delicat <[email protected]>

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

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<int> The vector of integers.
+ *
+ * @throw BT::RuntimeError Throws when there is no input or cannot parse int.
+ */
+template <>
+inline std::vector<int> convertFromString<std::vector<int>>(StringView str)
+{
+  auto parts = BT::splitString(str, ';');
+  if (!parts.size()) {
+    throw BT::RuntimeError("invalid input");
+  } else {
+    std::vector<int> result;
+    for (auto & part : parts) {
+      result.push_back(convertFromString<int>(part));
+    }
+
+    return result;
+  }
+}
+}  // namespace BT
+
 #endif  // PANTHER_MANAGER_BEHAVIOR_TREE_UTILS_HPP_

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 <memory>
+#include <mutex>
+#include <string>
+
+#include <behaviortree_ros2/bt_topic_sub_node.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <sensor_msgs/msg/joy.hpp>
+
+#include "panther_manager/behavior_tree_utils.hpp"
+
+namespace panther_manager
+{
+
+class AreButtonsPressed : public BT::RosTopicSubNode<sensor_msgs::msg::Joy>
+{
+public:
+  AreButtonsPressed(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+  : BT::RosTopicSubNode<sensor_msgs::msg::Joy>(name, conf, params)
+  {
+  }
+
+  BT::NodeStatus onTick(const std::shared_ptr<sensor_msgs::msg::Joy> & last_msg);
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts(
+      {BT::InputPort<std::vector<int>>("buttons", "state of buttons to accept a condition")});
+  }
+
+private:
+  std::vector<int> buttons_;
+};
+
+}  // namespace panther_manager
+
+#endif  // PANTHER_MANAGER_PLUGINS_CONDITION_ARE_BUTTONS_PRESSED_HPP_

panther_manager/package.xml

@@ -26,6 +26,7 @@
   <depend>panther_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
+  <depend>sensor_msgs</depend>
   <depend>std_srvs</depend>
   <depend>yaml-cpp</depend>
 

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<sensor_msgs::msg::Joy> & last_msg)
+{
+  getInput<std::vector<int>>("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");

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;
 }

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;
 }

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;
 }

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;
 }

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;
 }

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;
 }

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 <cstdint>
+#include <map>
+#include <string>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include <behaviortree_cpp/bt_factory.h>
+#include <rclcpp/rclcpp.hpp>
+
+#include <sensor_msgs/msg/joy.hpp>
+
+#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<int> & buttons);
+
+protected:
+  rclcpp::Publisher<sensor_msgs::msg::Joy>::SharedPtr joy_publisher_;
+  std::map<std::string, std::string> params_ = {{"topic_name", "joy"}, {"buttons", "0;1;0"}};
+};
+
+TestAreButtonsPressed::TestAreButtonsPressed()
+{
+  RegisterNodeWithParams<panther_manager::AreButtonsPressed>("AreButtonsPressed");
+  joy_publisher_ = bt_node_->create_publisher<sensor_msgs::msg::Joy>("joy", 10);
+}
+
+void TestAreButtonsPressed::PublishJoyMessage(const std::vector<int> & 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;
+}

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;
 }

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;
 }

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;
 }