diff --git a/husarion_ugv/hardware_deps.repos b/husarion_ugv/hardware_deps.repos
index 77ced26e1..abcbc820f 100644
--- a/husarion_ugv/hardware_deps.repos
+++ b/husarion_ugv/hardware_deps.repos
@@ -10,7 +10,7 @@ repositories:
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git
-    version: b29f5637a243b0008ac197032575c8df47883b2c
+    version: 517350ac672b7383c3377e63244353a104096c39
   ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity
     type: git
     url: https://github.com/husarion/ros2_controllers/
diff --git a/husarion_ugv/simulation_deps.repos b/husarion_ugv/simulation_deps.repos
index 83bab93e8..8ecbfcd64 100644
--- a/husarion_ugv/simulation_deps.repos
+++ b/husarion_ugv/simulation_deps.repos
@@ -14,7 +14,7 @@ repositories:
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git
-    version: b29f5637a243b0008ac197032575c8df47883b2c
+    version: 517350ac672b7383c3377e63244353a104096c39
   ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity
     type: git
     url: https://github.com/husarion/ros2_controllers/
diff --git a/husarion_ugv_manager/CMakeLists.txt b/husarion_ugv_manager/CMakeLists.txt
index fc9dda2ec..a9bb61910 100644
--- a/husarion_ugv_manager/CMakeLists.txt
+++ b/husarion_ugv_manager/CMakeLists.txt
@@ -12,12 +12,15 @@ set(PACKAGE_DEPENDENCIES
     behaviortree_ros2
     generate_parameter_library
     libssh
+    geometry_msgs
     husarion_ugv_msgs
     husarion_ugv_utils
     rclcpp
     rclcpp_action
     sensor_msgs
+    std_msgs
     std_srvs
+    tf2_geometry_msgs
     yaml-cpp)
 
 foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES})
@@ -52,6 +55,14 @@ 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(check_bool_msg_bt_node SHARED
+            src/plugins/condition/check_bool_msg.cpp)
+list(APPEND plugin_libs check_bool_msg_bt_node)
+
+add_library(check_joy_msg_bt_node SHARED
+            src/plugins/condition/check_joy_msg.cpp)
+list(APPEND plugin_libs check_joy_msg_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)
@@ -66,11 +77,13 @@ add_executable(safety_manager_node src/safety_manager_node_main.cpp
 ament_target_dependencies(
   safety_manager_node
   behaviortree_ros2
+  geometry_msgs
   husarion_ugv_msgs
   husarion_ugv_utils
   rclcpp
   sensor_msgs
-  std_msgs)
+  std_msgs
+  tf2_geometry_msgs)
 
 generate_parameter_library(safety_manager_parameters
                            config/safety_manager_parameters.yaml)
@@ -82,11 +95,13 @@ add_executable(lights_manager_node src/lights_manager_node_main.cpp
 ament_target_dependencies(
   lights_manager_node
   behaviortree_ros2
+  geometry_msgs
   husarion_ugv_msgs
   husarion_ugv_utils
   rclcpp
   sensor_msgs
-  std_msgs)
+  std_msgs
+  tf2_geometry_msgs)
 
 generate_parameter_library(lights_manager_parameters
                            config/lights_manager_parameters.yaml)
@@ -102,6 +117,7 @@ install(DIRECTORY behavior_trees config launch
         DESTINATION share/${PROJECT_NAME})
 
 install(DIRECTORY include/ DESTINATION include/)
+install(DIRECTORY test/ DESTINATION include/${PROJECT_NAME}/test/)
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
@@ -139,6 +155,18 @@ 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_check_bool_msg
+    test/plugins/condition/test_check_bool_msg.cpp
+    src/plugins/condition/check_bool_msg.cpp)
+  list(APPEND plugin_tests ${PROJECT_NAME}_test_check_bool_msg)
+
+  ament_add_gtest(
+    ${PROJECT_NAME}_test_check_joy_msg
+    test/plugins/condition/test_check_joy_msg.cpp
+    src/plugins/condition/check_joy_msg.cpp)
+  list(APPEND plugin_tests ${PROJECT_NAME}_test_check_joy_msg)
+
   ament_add_gtest(
     ${PROJECT_NAME}_test_tick_after_timeout_node
     test/plugins/decorator/test_tick_after_timeout_node.cpp
@@ -181,7 +209,7 @@ if(BUILD_TESTING)
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(
     ${PROJECT_NAME}_test_behavior_tree_utils behaviortree_cpp behaviortree_ros2
-    husarion_ugv_utils)
+    husarion_ugv_utils geometry_msgs tf2_geometry_msgs)
 
   ament_add_gtest(${PROJECT_NAME}_test_behavior_tree_manager
                   test/test_behavior_tree_manager.cpp)
@@ -202,11 +230,13 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_lights_manager_node
     behaviortree_cpp
     behaviortree_ros2
+    geometry_msgs
     husarion_ugv_msgs
     husarion_ugv_utils
     rclcpp
     sensor_msgs
-    std_msgs)
+    std_msgs
+    tf2_geometry_msgs)
   target_link_libraries(${PROJECT_NAME}_test_lights_manager_node
                         lights_manager_parameters)
 
@@ -221,11 +251,13 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_lights_behavior_tree
     behaviortree_cpp
     behaviortree_ros2
+    geometry_msgs
     husarion_ugv_msgs
     husarion_ugv_utils
     rclcpp
     sensor_msgs
-    std_msgs)
+    std_msgs
+    tf2_geometry_msgs)
   target_link_libraries(${PROJECT_NAME}_test_lights_behavior_tree
                         lights_manager_parameters)
 
@@ -239,11 +271,13 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_safety_manager_node
     behaviortree_cpp
     behaviortree_ros2
+    geometry_msgs
     husarion_ugv_msgs
     husarion_ugv_utils
     rclcpp
     sensor_msgs
-    std_msgs)
+    std_msgs
+    tf2_geometry_msgs)
   target_link_libraries(${PROJECT_NAME}_test_safety_manager_node
                         safety_manager_parameters)
 
@@ -258,12 +292,14 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_safety_behavior_tree
     behaviortree_cpp
     behaviortree_ros2
+    geometry_msgs
     husarion_ugv_msgs
     husarion_ugv_utils
     rclcpp
     sensor_msgs
     std_msgs
-    std_srvs)
+    std_srvs
+    tf2_geometry_msgs)
   target_link_libraries(${PROJECT_NAME}_test_safety_behavior_tree
                         safety_manager_parameters)
 endif()
diff --git a/husarion_ugv_manager/config/lights_manager.yaml b/husarion_ugv_manager/config/lights_manager.yaml
index cea2a5e4c..f9033f68e 100644
--- a/husarion_ugv_manager/config/lights_manager.yaml
+++ b/husarion_ugv_manager/config/lights_manager.yaml
@@ -2,6 +2,7 @@
   lights_manager:
     ros__parameters:
       timer_frequency: 10.0
+      bt_server_port: 5555
       battery:
         percent:
           window_len: 6
diff --git a/husarion_ugv_manager/config/lights_manager_parameters.yaml b/husarion_ugv_manager/config/lights_manager_parameters.yaml
index efe28cfdc..70f2afe53 100644
--- a/husarion_ugv_manager/config/lights_manager_parameters.yaml
+++ b/husarion_ugv_manager/config/lights_manager_parameters.yaml
@@ -44,6 +44,12 @@ lights_manager:
     default_value: ""
     description: Path to a BehaviorTree project.
 
+  bt_server_port:
+    type: int
+    default_value: 5555
+    description: Port number for the BehaviorTree server.
+    validation: { gt<>: 0 }
+
   plugin_libs:
     type: string_array
     default_value: []
diff --git a/husarion_ugv_manager/config/safety_manager.yaml b/husarion_ugv_manager/config/safety_manager.yaml
index d82d45c7b..98347112c 100644
--- a/husarion_ugv_manager/config/safety_manager.yaml
+++ b/husarion_ugv_manager/config/safety_manager.yaml
@@ -2,6 +2,7 @@
   safety_manager:
     ros__parameters:
       timer_frequency: 10.0
+      bt_server_port: 6666
       fan_turn_off_timeout: 60.0
       battery:
         temp:
diff --git a/husarion_ugv_manager/config/safety_manager_parameters.yaml b/husarion_ugv_manager/config/safety_manager_parameters.yaml
index 068a4d9d0..b064b1a6d 100644
--- a/husarion_ugv_manager/config/safety_manager_parameters.yaml
+++ b/husarion_ugv_manager/config/safety_manager_parameters.yaml
@@ -12,6 +12,12 @@ safety_manager:
     default_value: ""
     description: Path to a BehaviorTree project.
 
+  bt_server_port:
+    type: int
+    default_value: 6666
+    description: Port number for the BehaviorTree server.
+    validation: { gt<>: 0 }
+
   cpu:
     temp:
       fan_off:
diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp
index 8d411136b..30d3de2b6 100644
--- a/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp
@@ -26,6 +26,9 @@
 #include "behaviortree_cpp/utils/shared_library.h"
 #include "behaviortree_ros2/plugins.hpp"
 
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
 namespace husarion_ugv_manager::behavior_tree_utils
 {
 
@@ -90,4 +93,78 @@ inline std::string GetLoggerPrefix(const std::string & bt_node_name)
 }
 }  // namespace husarion_ugv_manager
 
+namespace BT
+{
+/**
+ * @brief Converts a string to a vector of float.
+ *
+ * @param str The string to convert.
+ * @return std::vector<float> The vector of float.
+ *
+ * @throw BT::RuntimeError Throws when there is no input or cannot parse float.
+ */
+template <>
+inline std::vector<float> convertFromString<std::vector<float>>(StringView str)
+{
+  auto parts = splitString(str, ';');
+  std::vector<float> output;
+  output.reserve(parts.size());
+  for (const StringView & part : parts) {
+    output.push_back(convertFromString<float>(part));
+  }
+  return output;
+}
+
+/**
+ * @brief Converts a string to a PoseStamped message.
+ *
+ * The string format should be "x;y;z;roll;pitch;yaw;frame_id" where:
+ *  - x, y, z: Position coordinates.
+ *  - roll, pitch, yaw: Rotation around axes XYZ.
+ *  - frame_id: Coordinate frame ID (string).
+ *
+ * @param str The string to convert.
+ * @return geometry_msgs::msg::PoseStamped The converted PoseStamped message.
+ *
+ * @throw BT::RuntimeError Throws if the input is invalid or cannot be parsed.
+ */
+template <>
+inline geometry_msgs::msg::PoseStamped convertFromString<geometry_msgs::msg::PoseStamped>(
+  StringView str)
+{
+  constexpr std::size_t expected_parts_size = 7;
+
+  auto parts = splitString(str, ';');
+  if (parts.size() != expected_parts_size) {
+    throw BT::RuntimeError(
+      "Invalid input for PoseStamped. Expected " + std::to_string(expected_parts_size) +
+      " values: x;y;z;roll;pitch;yaw;frame_id");
+  }
+
+  geometry_msgs::msg::PoseStamped pose_stamped;
+
+  try {
+    pose_stamped.pose.position.x = convertFromString<double>(parts[0]);
+    pose_stamped.pose.position.y = convertFromString<double>(parts[1]);
+    pose_stamped.pose.position.z = convertFromString<double>(parts[2]);
+
+    const auto roll = convertFromString<double>(parts[3]);
+    const auto pitch = convertFromString<double>(parts[4]);
+    const auto yaw = convertFromString<double>(parts[5]);
+    tf2::Quaternion quaternion;
+    quaternion.setRPY(roll, pitch, yaw);
+    pose_stamped.pose.orientation = tf2::toMsg(quaternion);
+
+    pose_stamped.header.frame_id = convertFromString<std::string>(parts[6]);
+    pose_stamped.header.stamp = rclcpp::Clock().now();
+
+  } catch (const std::exception & e) {
+    throw BT::RuntimeError("Failed to convert string to PoseStamped: " + std::string(e.what()));
+  }
+
+  return pose_stamped;
+}
+
+}  // namespace BT
+
 #endif  // HUSARION_UGV_MANAGER_BEHAVIOR_TREE_UTILS_HPP_
diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp
index f9bfae2ee..95278052d 100644
--- a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp
@@ -36,7 +36,8 @@ class CallSetBoolService : public BT::RosServiceNode<std_srvs::srv::SetBool>
 
   static BT::PortsList providedPorts()
   {
-    return providedBasicPorts({BT::InputPort<bool>("data", "true / false value")});
+    return providedBasicPorts(
+      {BT::InputPort<bool>("data", "Boolean value to send with the service request.")});
   }
 
   virtual bool setRequest(typename Request::SharedPtr & request) override;
diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp
index 5bf4d25cf..e033017f0 100644
--- a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp
@@ -37,11 +37,11 @@ class CallSetLedAnimationService
 
   static BT::PortsList providedPorts()
   {
-    return providedBasicPorts({
-      BT::InputPort<unsigned>("id", "animation ID"),
-      BT::InputPort<std::string>("param", "optional parameter"),
-      BT::InputPort<bool>("repeating", "indicates if animation should repeat"),
-    });
+    return providedBasicPorts(
+      {BT::InputPort<unsigned>("id", "Animation ID to trigger."),
+       BT::InputPort<std::string>("param", "Optional animation parameter."),
+       BT::InputPort<bool>(
+         "repeating", "Specifies whether the animation should repeated continuously.")});
   }
 
   virtual bool setRequest(typename Request::SharedPtr & request) override;
diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp
index 10396a8c2..7945bd8b5 100644
--- a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp
@@ -39,10 +39,8 @@ class ShutdownHostsFromFile : public ShutdownHosts
 
   static BT::PortsList providedPorts()
   {
-    return {
-      BT::InputPort<std::string>(
-        "shutdown_hosts_file", "global path to YAML file with hosts to shutdown"),
-    };
+    return {BT::InputPort<std::string>(
+      "shutdown_hosts_file", "Absolute path to a YAML file listing the hosts to shut down.")};
   }
 
 private:
diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp
index be36a7f96..e5ae3b798 100644
--- a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp
@@ -38,14 +38,17 @@ class ShutdownSingleHost : public ShutdownHosts
   static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<std::string>("ip", "ip of the host to shutdown"),
-      BT::InputPort<std::string>("username", "user to log into while executing shutdown command"),
-      BT::InputPort<unsigned>("port", "SSH communication port"),
-      BT::InputPort<std::string>("command", "command to execute on shutdown"),
-      BT::InputPort<float>("timeout", "time in seconds to wait for host to shutdown"),
+      BT::InputPort<std::string>("ip", "IP address of the host to shut down."),
+      BT::InputPort<std::string>(
+        "username", "Username to use for logging in and executing the shutdown command."),
+      BT::InputPort<unsigned>("port", "SSH port used for communication (default is usually 22)."),
+      BT::InputPort<std::string>("command", "A command to execute on the remote host."),
+      BT::InputPort<float>(
+        "timeout", "Maximum time (in seconds) to wait for the host to shut down."),
       BT::InputPort<bool>(
-        "ping_for_success", "ping host until it is not available or timeout is reached"),
-    };
+        "ping_for_success",
+        "Whether to continuously ping the host until it becomes unreachable or the timeout is "
+        "reached.")};
   }
 
 private:
diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp
index 8e98387e0..0e14dc4bb 100644
--- a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp
@@ -35,7 +35,7 @@ class SignalShutdown : public BT::SyncActionNode
   static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<std::string>("reason", "", "reason to shutdown robot"),
+      BT::InputPort<std::string>("reason", "", "A reason to shutdown a robot."),
     };
   }
 
diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.hpp
new file mode 100644
index 000000000..d99e6dd4c
--- /dev/null
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.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 HUSARION_UGV_MANAGER_HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_BOOL_MSG_HPP_
+#define HUSARION_UGV_MANAGER_HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_BOOL_MSG_HPP_
+
+#include <memory>
+#include <mutex>
+#include <string>
+
+#include <behaviortree_ros2/bt_topic_sub_node.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <std_msgs/msg/bool.hpp>
+
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
+
+namespace husarion_ugv_manager
+{
+
+// FIXME: There is no possibility to set QoS profile. Add it in the future to subscribe e_stop.
+class CheckBoolMsg : public BT::RosTopicSubNode<std_msgs::msg::Bool>
+{
+  using BoolMsg = std_msgs::msg::Bool;
+
+public:
+  CheckBoolMsg(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+  : BT::RosTopicSubNode<BoolMsg>(name, conf, params)
+  {
+  }
+
+  BT::NodeStatus onTick(const BoolMsg::SharedPtr & last_msg);
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts(
+      {BT::InputPort<bool>("data", "Specifies the expected state of the data field.")});
+  }
+};
+
+}  // namespace husarion_ugv_manager
+
+#endif  // HUSARION_UGV_MANAGER_HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_BOOL_MSG_HPP_
diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_joy_msg.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_joy_msg.hpp
new file mode 100644
index 000000000..c288cffa1
--- /dev/null
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_joy_msg.hpp
@@ -0,0 +1,68 @@
+// 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_HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_
+#define HUSARION_UGV_MANAGER_HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_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 "husarion_ugv_manager/behavior_tree_utils.hpp"
+
+namespace husarion_ugv_manager
+{
+
+class CheckJoyMsg : public BT::RosTopicSubNode<sensor_msgs::msg::Joy>
+{
+  using JoyMsg = sensor_msgs::msg::Joy;
+
+public:
+  CheckJoyMsg(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+  : BT::RosTopicSubNode<JoyMsg>(name, conf, params)
+  {
+  }
+
+  BT::NodeStatus onTick(const JoyMsg::SharedPtr & last_msg);
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts(
+      {BT::InputPort<std::vector<float>>(
+         "axes", "",
+         "Specifies the expected state of the axes field. An empty string (\"\") means the values "
+         "are ignored."),
+       BT::InputPort<std::vector<int>>(
+         "buttons", "",
+         "Specifies the expected state of the buttons field. An empty string (\"\") means values "
+         "are ignored."),
+       BT::InputPort<double>(
+         "timeout", 0.0, "Maximum allowable time delay to accept the condition.")});
+  }
+
+private:
+  bool checkAxes(const JoyMsg::SharedPtr & last_msg);
+  bool checkButtons(const JoyMsg::SharedPtr & last_msg);
+  bool checkTimeout(const JoyMsg::SharedPtr & last_msg);
+};
+
+}  // namespace husarion_ugv_manager
+
+#endif  // HUSARION_UGV_MANAGER_HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_
diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp
index a453054f0..41bca877d 100644
--- a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp
+++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp
@@ -31,7 +31,8 @@ class TickAfterTimeout : public BT::DecoratorNode
 
   static BT::PortsList providedPorts()
   {
-    return {BT::InputPort<float>("timeout", "time in s to wait before ticking child again")};
+    return {BT::InputPort<float>(
+      "timeout", "Time in seconds to wait before ticking the child node again.")};
   }
 
 private:
diff --git a/husarion_ugv_manager/package.xml b/husarion_ugv_manager/package.xml
index a298fbb4d..46445054f 100644
--- a/husarion_ugv_manager/package.xml
+++ b/husarion_ugv_manager/package.xml
@@ -20,6 +20,7 @@
   <depend>behaviortree_cpp</depend>
   <depend>behaviortree_ros2</depend>
   <depend>generate_parameter_library</depend>
+  <depend>geometry_msgs</depend>
   <depend>husarion_ugv_msgs</depend>
   <depend>husarion_ugv_utils</depend>
   <depend>iputils-ping</depend>
@@ -27,7 +28,9 @@
   <depend>libssh-dev</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
+  <depend>sensor_msgs</depend>
   <depend>std_srvs</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>yaml-cpp</depend>
 
   <test_depend>ament_lint_auto</test_depend>
diff --git a/husarion_ugv_manager/src/lights_manager_node.cpp b/husarion_ugv_manager/src/lights_manager_node.cpp
index 2238ded13..9c126633c 100644
--- a/husarion_ugv_manager/src/lights_manager_node.cpp
+++ b/husarion_ugv_manager/src/lights_manager_node.cpp
@@ -48,8 +48,10 @@ LightsManagerNode::LightsManagerNode(
   battery_percent_ma_ = std::make_unique<husarion_ugv_utils::MovingAverage<double>>(
     battery_percent_window_len, 1.0);
 
+  const auto bt_server_port = this->get_parameter("bt_server_port").as_int();
   const auto initial_blackboard = CreateLightsInitialBlackboard();
-  lights_tree_manager_ = std::make_unique<BehaviorTreeManager>("Lights", initial_blackboard, 5555);
+  lights_tree_manager_ = std::make_unique<BehaviorTreeManager>(
+    "Lights", initial_blackboard, bt_server_port);
 
   RCLCPP_INFO(this->get_logger(), "Node constructed successfully.");
 }
@@ -70,11 +72,10 @@ void LightsManagerNode::Initialize()
     std::bind(&LightsManagerNode::EStopCB, this, _1));
 
   const double timer_freq = this->params_.timer_frequency;
-  const auto timer_period_ms =
-    std::chrono::milliseconds(static_cast<unsigned>(1.0f / timer_freq * 1000));
+  const auto timer_period = std::chrono::duration<double>(1.0 / timer_freq);
 
   lights_tree_timer_ = this->create_wall_timer(
-    timer_period_ms, std::bind(&LightsManagerNode::LightsTreeTimerCB, this));
+    timer_period, std::bind(&LightsManagerNode::LightsTreeTimerCB, this));
 
   RCLCPP_INFO(this->get_logger(), "Initialized successfully.");
 }
@@ -91,16 +92,17 @@ void LightsManagerNode::RegisterBehaviorTree()
 
   BT::RosNodeParams params;
   params.nh = this->shared_from_this();
+  auto wait_for_server_timeout_s = std::chrono::duration<double>(service_availability_timeout);
   params.wait_for_server_timeout =
-    std::chrono::milliseconds(static_cast<int>(service_availability_timeout * 1000));
-  params.server_timeout =
-    std::chrono::milliseconds(static_cast<int>(service_response_timeout * 1000));
+    std::chrono::duration_cast<std::chrono::milliseconds>(wait_for_server_timeout_s);
+  auto server_timeout_s = std::chrono::duration<double>(service_response_timeout);
+  params.server_timeout = std::chrono::duration_cast<std::chrono::milliseconds>(server_timeout_s);
 
   behavior_tree_utils::RegisterBehaviorTree(
     factory_, bt_project_path, plugin_libs, params, ros_plugin_libs);
 
-  RCLCPP_INFO(
-    this->get_logger(), "BehaviorTree registered from path '%s'", bt_project_path.c_str());
+  RCLCPP_INFO_STREAM(
+    this->get_logger(), "BehaviorTree registered from path '" << bt_project_path << "'");
 }
 
 std::map<std::string, std::any> LightsManagerNode::CreateLightsInitialBlackboard()
@@ -144,7 +146,6 @@ std::map<std::string, std::any> LightsManagerNode::CreateLightsInitialBlackboard
     {"POWER_SUPPLY_STATUS_FULL", unsigned(BatteryStateMsg::POWER_SUPPLY_STATUS_FULL)},
     // battery health constants
     {"POWER_SUPPLY_HEALTH_OVERHEAT", unsigned(BatteryStateMsg::POWER_SUPPLY_HEALTH_OVERHEAT)},
-
   };
 
   RCLCPP_INFO(this->get_logger(), "Blackboard created.");
diff --git a/husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp b/husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp
new file mode 100644
index 000000000..892d03810
--- /dev/null
+++ b/husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp
@@ -0,0 +1,34 @@
+// 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/condition/check_bool_msg.hpp"
+
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
+
+namespace husarion_ugv_manager
+{
+
+BT::NodeStatus CheckBoolMsg::onTick(const BoolMsg::SharedPtr & last_msg)
+{
+  bool expected_data;
+  getInput<bool>("data", expected_data);
+
+  return (last_msg && last_msg->data == expected_data) ? BT::NodeStatus::SUCCESS
+                                                       : BT::NodeStatus::FAILURE;
+}
+
+}  // namespace husarion_ugv_manager
+
+#include "behaviortree_ros2/plugins.hpp"
+CreateRosNodePlugin(husarion_ugv_manager::CheckBoolMsg, "CheckBoolMsg");
diff --git a/husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp b/husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp
new file mode 100644
index 000000000..8d28cbe1e
--- /dev/null
+++ b/husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp
@@ -0,0 +1,92 @@
+// 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/condition/check_joy_msg.hpp"
+
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
+
+namespace husarion_ugv_manager
+{
+
+BT::NodeStatus CheckJoyMsg::onTick(const JoyMsg::SharedPtr & last_msg)
+{
+  return (last_msg && checkTimeout(last_msg) && checkAxes(last_msg) && checkButtons(last_msg))
+           ? BT::NodeStatus::SUCCESS
+           : BT::NodeStatus::FAILURE;
+}
+
+bool CheckJoyMsg::checkAxes(const JoyMsg::SharedPtr & last_msg)
+{
+  std::vector<float> expected_axes;
+  getInput<std::vector<float>>("axes", expected_axes);
+
+  if (expected_axes.empty()) {
+    return true;
+  }
+
+  if (last_msg->axes.size() != expected_axes.size()) {
+    RCLCPP_WARN_STREAM(
+      this->logger(), GetLoggerPrefix(name())
+                        << "Joy message has " << last_msg->axes.size()
+                        << " axes, expected at least " << expected_axes.size());
+    return false;
+  }
+
+  return std::equal(
+    expected_axes.begin(), expected_axes.end(), last_msg->axes.begin(),
+    [](float a, float b) { return std::fabs(a - b) <= std::numeric_limits<float>::epsilon(); });
+}
+
+bool CheckJoyMsg::checkButtons(const JoyMsg::SharedPtr & last_msg)
+{
+  std::vector<int> expected_buttons;
+  getInput<std::vector<int>>("buttons", expected_buttons);
+
+  if (expected_buttons.empty()) {
+    return true;
+  }
+
+  if (last_msg->buttons.size() != expected_buttons.size()) {
+    RCLCPP_WARN_STREAM(
+      this->logger(), GetLoggerPrefix(name())
+                        << "Joy message has " << last_msg->buttons.size()
+                        << " buttons, expected at least " << expected_buttons.size());
+    return false;
+  }
+
+  return std::equal(expected_buttons.begin(), expected_buttons.end(), last_msg->buttons.begin());
+}
+
+bool CheckJoyMsg::checkTimeout(const JoyMsg::SharedPtr & last_msg)
+{
+  double max_timeout;
+  getInput<double>("timeout", max_timeout);
+
+  rclcpp::Time now;
+  {
+    auto node = node_.lock();
+    if (!node) {
+      return false;
+    }
+    now = node->now();
+  }
+
+  const auto time_diff = now - last_msg->header.stamp;
+  return (max_timeout <= 0.0) || time_diff < rclcpp::Duration::from_seconds(max_timeout);
+}
+
+}  // namespace husarion_ugv_manager
+
+#include "behaviortree_ros2/plugins.hpp"
+CreateRosNodePlugin(husarion_ugv_manager::CheckJoyMsg, "CheckJoyMsg");
diff --git a/husarion_ugv_manager/src/safety_manager_node.cpp b/husarion_ugv_manager/src/safety_manager_node.cpp
index 33ede2c03..bdfd3ee97 100644
--- a/husarion_ugv_manager/src/safety_manager_node.cpp
+++ b/husarion_ugv_manager/src/safety_manager_node.cpp
@@ -93,16 +93,11 @@ void SafetyManagerNode::Initialize()
   system_status_sub_ = this->create_subscription<SystemStatusMsg>(
     "system_status", 10, std::bind(&SafetyManagerNode::SystemStatusCB, this, _1));
 
-  // -------------------------------
-  //   Timers
-  // -------------------------------
-
   const double timer_freq = this->params_.timer_frequency;
-  const auto timer_period_ms =
-    std::chrono::milliseconds(static_cast<unsigned>(1.0f / timer_freq * 1000));
+  const auto timer_period = std::chrono::duration<double>(1.0 / timer_freq);
 
   safety_tree_timer_ = this->create_wall_timer(
-    timer_period_ms, std::bind(&SafetyManagerNode::SafetyTreeTimerCB, this));
+    timer_period, std::bind(&SafetyManagerNode::SafetyTreeTimerCB, this));
 
   RCLCPP_INFO(this->get_logger(), "Initialized successfully.");
 }
@@ -119,16 +114,17 @@ void SafetyManagerNode::RegisterBehaviorTree()
 
   BT::RosNodeParams params;
   params.nh = this->shared_from_this();
+  auto wait_for_server_timeout_s = std::chrono::duration<double>(service_availability_timeout);
   params.wait_for_server_timeout =
-    std::chrono::milliseconds(static_cast<int>(service_availability_timeout * 1000));
-  params.server_timeout =
-    std::chrono::milliseconds(static_cast<int>(service_response_timeout * 1000));
+    std::chrono::duration_cast<std::chrono::milliseconds>(wait_for_server_timeout_s);
+  auto server_timeout_s = std::chrono::duration<double>(service_response_timeout);
+  params.server_timeout = std::chrono::duration_cast<std::chrono::milliseconds>(server_timeout_s);
 
   behavior_tree_utils::RegisterBehaviorTree(
     factory_, bt_project_path, plugin_libs, params, ros_plugin_libs);
 
-  RCLCPP_INFO(
-    this->get_logger(), "BehaviorTree registered from path '%s'", bt_project_path.c_str());
+  RCLCPP_INFO_STREAM(
+    this->get_logger(), "BehaviorTree registered from path '" << bt_project_path << "'");
 }
 
 std::map<std::string, std::any> SafetyManagerNode::CreateSafetyInitialBlackboard()
diff --git a/husarion_ugv_manager/test/plugins/action/test_call_set_bool_service_node.cpp b/husarion_ugv_manager/test/plugins/action/test_call_set_bool_service_node.cpp
index e359bc59d..d8ec8358f 100644
--- a/husarion_ugv_manager/test/plugins/action/test_call_set_bool_service_node.cpp
+++ b/husarion_ugv_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/husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp b/husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp
index 2c3969c8c..5c8ed3b4f 100644
--- a/husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp
+++ b/husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp
@@ -183,5 +183,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/husarion_ugv_manager/test/plugins/action/test_call_trigger_service_node.cpp b/husarion_ugv_manager/test/plugins/action/test_call_trigger_service_node.cpp
index a130e5308..5acbb291c 100644
--- a/husarion_ugv_manager/test/plugins/action/test_call_trigger_service_node.cpp
+++ b/husarion_ugv_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/husarion_ugv_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp b/husarion_ugv_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp
index 980f9e0af..f46df83f6 100644
--- a/husarion_ugv_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp
+++ b/husarion_ugv_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp
@@ -106,5 +106,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/husarion_ugv_manager/test/plugins/action/test_shutdown_single_host_node.cpp b/husarion_ugv_manager/test/plugins/action/test_shutdown_single_host_node.cpp
index 5ed922dc9..e14a30cc0 100644
--- a/husarion_ugv_manager/test/plugins/action/test_shutdown_single_host_node.cpp
+++ b/husarion_ugv_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/husarion_ugv_manager/test/plugins/action/test_signal_shutdown_node.cpp b/husarion_ugv_manager/test/plugins/action/test_signal_shutdown_node.cpp
index 3b0a383b9..11b21994a 100644
--- a/husarion_ugv_manager/test/plugins/action/test_signal_shutdown_node.cpp
+++ b/husarion_ugv_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/husarion_ugv_manager/test/plugins/condition/test_check_bool_msg.cpp b/husarion_ugv_manager/test/plugins/condition/test_check_bool_msg.cpp
new file mode 100644
index 000000000..a18920442
--- /dev/null
+++ b/husarion_ugv_manager/test/plugins/condition/test_check_bool_msg.cpp
@@ -0,0 +1,102 @@
+// 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 <memory>
+#include <string>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include <behaviortree_cpp/bt_factory.h>
+#include <rclcpp/rclcpp.hpp>
+
+#include <std_msgs/msg/bool.hpp>
+
+#include "husarion_ugv_manager/plugins/condition/check_bool_msg.hpp"
+#include "utils/plugin_test_utils.hpp"
+
+using BoolMsg = std_msgs::msg::Bool;
+using bt_ports = std::map<std::string, std::string>;
+
+struct TestCase
+{
+  BT::NodeStatus result;
+  bt_ports input;
+  BoolMsg msg;
+};
+
+constexpr auto TOPIC = "bool";
+constexpr auto PLUGIN = "CheckBoolMsg";
+
+class TestCheckBoolMsg : public husarion_ugv_manager::plugin_test_utils::PluginTestUtils
+{
+public:
+  TestCheckBoolMsg();
+  BoolMsg CreateMsg(bool data);
+  void PublishMsg(BoolMsg msg) { publisher_->publish(msg); }
+
+protected:
+  rclcpp::Publisher<BoolMsg>::SharedPtr publisher_;
+};
+
+TestCheckBoolMsg::TestCheckBoolMsg()
+{
+  RegisterNodeWithParams<husarion_ugv_manager::CheckBoolMsg>(PLUGIN);
+  publisher_ = bt_node_->create_publisher<BoolMsg>(TOPIC, 10);
+}
+
+BoolMsg TestCheckBoolMsg::CreateMsg(bool data)
+{
+  BoolMsg msg;
+  msg.data = data;
+  return msg;
+}
+
+TEST_F(TestCheckBoolMsg, NoMessageArrived)
+{
+  bt_ports input = {{"topic_name", TOPIC}, {"data", "true"}};
+  ASSERT_NO_THROW({ CreateTree(PLUGIN, input); });
+
+  auto & tree = GetTree();
+  auto status = tree.tickWhileRunning();
+  EXPECT_EQ(status, BT::NodeStatus::FAILURE);
+}
+
+TEST_F(TestCheckBoolMsg, OnTickBehavior)
+{
+  std::vector<TestCase> test_cases = {
+    {BT::NodeStatus::SUCCESS, {{"topic_name", TOPIC}, {"data", "true"}}, CreateMsg(true)},
+    {BT::NodeStatus::SUCCESS, {{"topic_name", TOPIC}, {"data", "false"}}, CreateMsg(false)},
+    {BT::NodeStatus::FAILURE, {{"topic_name", TOPIC}, {"data", "true"}}, CreateMsg(false)},
+    {BT::NodeStatus::FAILURE, {{"topic_name", TOPIC}, {"data", "false"}}, CreateMsg(true)}};
+
+  for (auto & test_case : test_cases) {
+    CreateTree(PLUGIN, test_case.input);
+    PublishMsg(test_case.msg);
+
+    auto & tree = GetTree();
+    auto status = tree.tickWhileRunning();
+
+    EXPECT_EQ(status, test_case.result);
+  }
+}
+
+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/condition/test_check_joy_msg.cpp b/husarion_ugv_manager/test/plugins/condition/test_check_joy_msg.cpp
new file mode 100644
index 000000000..cfb0478f4
--- /dev/null
+++ b/husarion_ugv_manager/test/plugins/condition/test_check_joy_msg.cpp
@@ -0,0 +1,186 @@
+// 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 <memory>
+#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 <std_msgs/msg/header.hpp>
+
+#include "husarion_ugv_manager/plugins/condition/check_joy_msg.hpp"
+#include "utils/plugin_test_utils.hpp"
+
+using JoyMsg = sensor_msgs::msg::Joy;
+using HeaderMsg = std_msgs::msg::Header;
+using bt_ports = std::map<std::string, std::string>;
+
+struct TestCase
+{
+  BT::NodeStatus result;
+  bt_ports input;
+  JoyMsg msg;
+};
+
+constexpr auto TOPIC = "joy";
+constexpr auto PLUGIN = "CheckJoyMsg";
+
+class TestCheckJoyMsg : public husarion_ugv_manager::plugin_test_utils::PluginTestUtils
+{
+public:
+  TestCheckJoyMsg();
+  void PublishMsg(JoyMsg msg) { joy_publisher_->publish(msg); };
+  JoyMsg CreateMsg(
+    const std::vector<float> & axes = {}, const std::vector<int> & buttons = {},
+    const HeaderMsg & header = HeaderMsg());
+  void SetCurrentMsgTime(JoyMsg & header);
+
+protected:
+  rclcpp::Publisher<JoyMsg>::SharedPtr joy_publisher_;
+};
+
+TestCheckJoyMsg::TestCheckJoyMsg()
+{
+  RegisterNodeWithParams<husarion_ugv_manager::CheckJoyMsg>(PLUGIN);
+  joy_publisher_ = bt_node_->create_publisher<JoyMsg>(TOPIC, 10);
+}
+
+JoyMsg TestCheckJoyMsg::CreateMsg(
+  const std::vector<float> & axes, const std::vector<int> & buttons, const HeaderMsg & header)
+{
+  JoyMsg msg;
+  msg.header = header;
+  msg.axes = axes;
+  msg.buttons = buttons;
+  return msg;
+}
+
+void TestCheckJoyMsg::SetCurrentMsgTime(JoyMsg & msg) { msg.header.stamp = bt_node_->now(); }
+
+TEST_F(TestCheckJoyMsg, NoMessageArrived)
+{
+  bt_ports input = {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}};
+  ASSERT_NO_THROW({ CreateTree(PLUGIN, input); });
+
+  auto & tree = GetTree();
+  auto status = tree.tickWhileRunning();
+  EXPECT_EQ(status, BT::NodeStatus::FAILURE);
+}
+
+TEST_F(TestCheckJoyMsg, TimeoutTests)
+{
+  std::vector<TestCase> test_cases = {
+    {BT::NodeStatus::SUCCESS,
+     {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", ""}, {"timeout", "0.5"}},
+     CreateMsg()},
+    {BT::NodeStatus::SUCCESS,
+     {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", ""}, {"timeout", "0.0"}},
+     CreateMsg()},
+    {BT::NodeStatus::SUCCESS,
+     {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", ""}, {"timeout", "-0.5"}},
+     CreateMsg()},
+    {BT::NodeStatus::FAILURE,
+     {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", ""}, {"timeout", "0.001"}},
+     CreateMsg()}};
+
+  for (auto & test_case : test_cases) {
+    CreateTree(PLUGIN, test_case.input);
+    SetCurrentMsgTime(test_case.msg);
+    std::this_thread::sleep_for(std::chrono::milliseconds(2));
+    PublishMsg(test_case.msg);
+
+    auto & tree = GetTree();
+    auto status = tree.tickWhileRunning();
+
+    EXPECT_EQ(status, test_case.result);
+  }
+}
+
+TEST_F(TestCheckJoyMsg, OnTickBehavior)
+{
+  std::vector<TestCase> test_cases = {
+    {BT::NodeStatus::SUCCESS,
+     {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", ""}, {"timeout", "1.0"}},
+     CreateMsg()},
+    {BT::NodeStatus::SUCCESS,
+     {{"topic_name", TOPIC}, {"axes", "1"}, {"buttons", ""}, {"timeout", "1.0"}},
+     CreateMsg({1})},
+    {BT::NodeStatus::SUCCESS,
+     {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", "1"}, {"timeout", "1.0"}},
+     CreateMsg({}, {1})},
+    {BT::NodeStatus::SUCCESS,
+     {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}},
+     CreateMsg({0, 0}, {0, 0})},
+    {BT::NodeStatus::SUCCESS,
+     {{"topic_name", TOPIC}, {"axes", "0.1;-0.2;1.0"}, {"buttons", "0;0;10;0"}, {"timeout", "1.0"}},
+     CreateMsg({0.1, -0.2, 1.0}, {0, 0, 10, 0})},
+    {BT::NodeStatus::SUCCESS,
+     {{"topic_name", TOPIC}, {"axes", "0.1;-0.2;1.0"}, {"buttons", ""}, {"timeout", "1.0"}},
+     CreateMsg({0.1, -0.2, 1.0}, {0, 0, 10, 0})},
+    {BT::NodeStatus::SUCCESS,
+     {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", "0;0;10;0"}, {"timeout", "1.0"}},
+     CreateMsg({0.1, -0.2, 1.0}, {0, 0, 10, 0})},
+    {BT::NodeStatus::FAILURE,
+     {{"topic_name", TOPIC}, {"axes", "0.1;-0.2;1.0"}, {"buttons", "0;0;10;0"}, {"timeout", "1.0"}},
+     CreateMsg({0.2, -0.2, 1.0}, {0, 0, 10, 0})},
+    {BT::NodeStatus::FAILURE,
+     {{"topic_name", TOPIC}, {"axes", "0;0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}},
+     CreateMsg({0, 0}, {0, 0})},
+    {BT::NodeStatus::FAILURE,
+     {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0;0"}, {"timeout", "1.0"}},
+     CreateMsg({0, 0}, {0, 0})},
+    {BT::NodeStatus::FAILURE,
+     {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}},
+     CreateMsg({0, 0, 0}, {0, 0})},
+    {BT::NodeStatus::FAILURE,
+     {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}},
+     CreateMsg({0, 0}, {0, 0, 0})},
+    {BT::NodeStatus::FAILURE,
+     {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}},
+     CreateMsg({0, 1}, {0, 0})},
+    {BT::NodeStatus::FAILURE,
+     {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}},
+     CreateMsg({0, 0}, {0, 1})},
+    {BT::NodeStatus::FAILURE,
+     {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}},
+     CreateMsg({1, 1}, {1, 1})},
+    {BT::NodeStatus::FAILURE,
+     {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0;0"}, {"timeout", "1.0"}},
+     CreateMsg({1, 1}, {1, 1, 1})}};
+
+  for (auto & test_case : test_cases) {
+    CreateTree(PLUGIN, test_case.input);
+    SetCurrentMsgTime(test_case.msg);
+    PublishMsg(test_case.msg);
+
+    auto & tree = GetTree();
+    auto status = tree.tickWhileRunning();
+
+    EXPECT_EQ(status, test_case.result);
+  }
+}
+
+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/decorator/test_tick_after_timeout_node.cpp b/husarion_ugv_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp
index fb2eca460..5e90e5dbc 100644
--- a/husarion_ugv_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp
+++ b/husarion_ugv_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/husarion_ugv_manager/test/plugins/test_shutdown_host.cpp b/husarion_ugv_manager/test/plugins/test_shutdown_host.cpp
index cd48e27d6..9b56fe416 100644
--- a/husarion_ugv_manager/test/plugins/test_shutdown_host.cpp
+++ b/husarion_ugv_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/husarion_ugv_manager/test/plugins/test_shutdown_hosts_node.cpp b/husarion_ugv_manager/test/plugins/test_shutdown_hosts_node.cpp
index eb7971107..324ad3e8c 100644
--- a/husarion_ugv_manager/test/plugins/test_shutdown_hosts_node.cpp
+++ b/husarion_ugv_manager/test/plugins/test_shutdown_hosts_node.cpp
@@ -159,5 +159,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/husarion_ugv_manager/test/test_behavior_tree_utils.cpp b/husarion_ugv_manager/test/test_behavior_tree_utils.cpp
index ac32b59e3..fde1d0b30 100644
--- a/husarion_ugv_manager/test/test_behavior_tree_utils.cpp
+++ b/husarion_ugv_manager/test/test_behavior_tree_utils.cpp
@@ -26,6 +26,8 @@
 #include "behaviortree_ros2/ros_node_params.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
 #include "husarion_ugv_manager/behavior_tree_utils.hpp"
 #include "husarion_ugv_utils/test/test_utils.hpp"
 
@@ -121,6 +123,61 @@ TEST_F(TestRegisterBT, RegisterBehaviorTreeROS)
   rclcpp::shutdown();
 }
 
+TEST(TestConvertFromStringPoseStamped, GoodInput)
+{
+  constexpr double time_threshold = 0.1;
+  constexpr float epsilon = 1e-3;
+
+  auto str = "1;2;3;0.1;0.2;0.1;pose";
+  auto result = BT::convertFromString<geometry_msgs::msg::PoseStamped>(str);
+
+  auto time_diff = rclcpp::Clock().now() - rclcpp::Time(result.header.stamp, RCL_SYSTEM_TIME);
+  EXPECT_LE(time_diff.seconds(), time_threshold);
+  EXPECT_EQ(result.header.frame_id, "pose");
+
+  EXPECT_NEAR(result.pose.position.x, 1, epsilon);
+  EXPECT_NEAR(result.pose.position.y, 2, epsilon);
+  EXPECT_NEAR(result.pose.position.z, 3, epsilon);
+
+  EXPECT_NEAR(result.pose.orientation.x, 0.0447, epsilon);
+  EXPECT_NEAR(result.pose.orientation.y, 0.1021, epsilon);
+  EXPECT_NEAR(result.pose.orientation.z, 0.0447, epsilon);
+  EXPECT_NEAR(result.pose.orientation.w, 0.9928, epsilon);
+}
+
+TEST(TestConvertFromStringPoseStamped, WrongInput)
+{
+  auto str = "";
+  EXPECT_THROW(BT::convertFromString<geometry_msgs::msg::PoseStamped>(str), BT::RuntimeError);
+  str = "1;2;3;0.1;0.2;0.1;";
+  EXPECT_THROW(BT::convertFromString<geometry_msgs::msg::PoseStamped>(str), BT::RuntimeError);
+  str = "1;2;3;0.1;0.2;0.1;pose;0.1";
+  EXPECT_THROW(BT::convertFromString<geometry_msgs::msg::PoseStamped>(str), BT::RuntimeError);
+  str = "pose;1;2;3;0.1;0.2;0.1;";
+  EXPECT_THROW(BT::convertFromString<geometry_msgs::msg::PoseStamped>(str), BT::RuntimeError);
+}
+
+TEST(TestConvertFromStringVectorOfDouble, GoodInput)
+{
+  constexpr float epsilon = 1e-6;
+
+  auto str = "1;2;3;0.1;0.2;0.1";
+  auto result = BT::convertFromString<std::vector<float>>(str);
+
+  EXPECT_NEAR(result[0], 1, epsilon);
+  EXPECT_NEAR(result[1], 2, epsilon);
+  EXPECT_NEAR(result[2], 3, epsilon);
+  EXPECT_NEAR(result[3], 0.1, epsilon);
+  EXPECT_NEAR(result[4], 0.2, epsilon);
+  EXPECT_NEAR(result[5], 0.1, epsilon);
+}
+
+TEST(TestConvertFromStringVectorOfFloat, WrongInput)
+{
+  auto str = "1;2;3;0.1;a;0.2";
+  EXPECT_THROW(BT::convertFromString<std::vector<float>>(str), std::invalid_argument);
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/husarion_ugv_manager/test/utils/plugin_test_utils.hpp b/husarion_ugv_manager/test/utils/plugin_test_utils.hpp
index 9b95e0a50..299cc12a9 100644
--- a/husarion_ugv_manager/test/utils/plugin_test_utils.hpp
+++ b/husarion_ugv_manager/test/utils/plugin_test_utils.hpp
@@ -26,6 +26,7 @@
 
 #include "behaviortree_cpp/bt_factory.h"
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
 
 #include "std_srvs/srv/set_bool.hpp"
 #include "std_srvs/srv/trigger.hpp"
@@ -106,10 +107,30 @@ class PluginTestUtils : public testing::Test
       void(const typename ServiceT::Request::SharedPtr, typename ServiceT::Response::SharedPtr)>
       service_callback)
   {
-    service_server_node_ = std::make_shared<rclcpp::Node>("test_node_for_" + service_name);
-    service = service_server_node_->create_service<ServiceT>(service_name, service_callback);
+    server_node_ = std::make_shared<rclcpp::Node>("test_node_for_" + service_name);
+    service_server_ = server_node_->create_service<ServiceT>(service_name, service_callback);
     executor_ = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
-    executor_->add_node(service_server_node_);
+    executor_->add_node(server_node_);
+    executor_thread_ = std::make_unique<std::thread>([this]() { executor_->spin(); });
+  }
+
+  template <typename ActionT>
+  void CreateAction(
+    const std::string & action_name,
+    std::function<rclcpp_action::GoalResponse(
+      const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const typename ActionT::Goal> goal)>
+      handle_goal,
+    std::function<rclcpp_action::CancelResponse(
+      const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle)>
+      handle_cancel,
+    std::function<void(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle)>
+      handle_accepted)
+  {
+    server_node_ = std::make_shared<rclcpp::Node>("test_node_for_" + action_name);
+    action_server_ = rclcpp_action::create_server<ActionT>(
+      server_node_, action_name, handle_goal, handle_cancel, handle_accepted);
+    executor_ = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
+    executor_->add_node(server_node_);
     executor_thread_ = std::make_unique<std::thread>([this]() { executor_->spin(); });
   }
 
@@ -133,10 +154,11 @@ class PluginTestUtils : public testing::Test
   BT::BehaviorTreeFactory factory_;
   BT::Tree tree_;
 
-  rclcpp::Node::SharedPtr service_server_node_;
+  rclcpp::Node::SharedPtr server_node_;
   rclcpp::executors::SingleThreadedExecutor::UniquePtr executor_;
 
-  rclcpp::ServiceBase::SharedPtr service;
+  rclcpp::ServiceBase::SharedPtr service_server_;
+  rclcpp_action::ServerBase::SharedPtr action_server_;
   std::unique_ptr<std::thread> executor_thread_;
 
   inline void SpinExecutor() { executor_->spin(); }
diff --git a/husarion_ugv_utils/include/husarion_ugv_utils/ros_utils.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/ros_utils.hpp
index 6d226ca7b..d2ecb7074 100644
--- a/husarion_ugv_utils/include/husarion_ugv_utils/ros_utils.hpp
+++ b/husarion_ugv_utils/include/husarion_ugv_utils/ros_utils.hpp
@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef HUSARION_UGV_UTILS__ROS_UTILS_HPP_
-#define HUSARION_UGV_UTILS__ROS_UTILS_HPP_
+#ifndef HUSARION_UGV_UTILS_HUSARION_UGV_UTILS_ROS_UTILS_HPP_
+#define HUSARION_UGV_UTILS_HUSARION_UGV_UTILS_ROS_UTILS_HPP_
 
 #include <chrono>
 
-#include "std_msgs/msg/header.hpp"
+#include <std_msgs/msg/header.hpp>
 
 namespace husarion_ugv_utils::ros
 {
@@ -90,6 +90,16 @@ std_msgs::msg::Header MergeHeaders(
   return merged_header;
 }
 
+/**
+ * @brief Adds a namespace to a frame ID.
+ *
+ * This function adds a namespace to a frame ID. The namespace is added as a prefix to the frame ID.
+ *
+ * @param frame_id The frame ID to which the namespace should be added.
+ * @param node_namespace The namespace to be added to the frame ID.
+ *
+ * @return The frame ID with the namespace added as a prefix.
+ */
 std::string AddNamespaceToFrameID(const std::string & frame_id, const std::string & node_namespace)
 {
   std::string tf_prefix = node_namespace;
@@ -107,4 +117,4 @@ std::string AddNamespaceToFrameID(const std::string & frame_id, const std::strin
 
 }  // namespace husarion_ugv_utils::ros
 
-#endif  // HUSARION_UGV_UTILS__ROS_UTILS_HPP_
+#endif  // HUSARION_UGV_UTILS_HUSARION_UGV_UTILS_ROS_UTILS_HPP_
diff --git a/husarion_ugv_utils/test/test_common_utilities.cpp b/husarion_ugv_utils/test/test_common_utilities.cpp
index fe3749361..0b9d06683 100644
--- a/husarion_ugv_utils/test/test_common_utilities.cpp
+++ b/husarion_ugv_utils/test/test_common_utilities.cpp
@@ -14,6 +14,7 @@
 
 #include <filesystem>
 #include <fstream>
+#include <limits>
 #include <map>
 #include <sstream>
 #include <string>
@@ -195,6 +196,17 @@ TEST(TestMeetsVersionRequirement, NaNVersionRequired)
   EXPECT_FALSE(is_met);
 }
 
+TEST(TestMeetsVersionRequirement, CorrectlyComparesVersions)
+{
+  float panther_version = 1.06;
+  EXPECT_TRUE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(panther_version, 0.0));
+  EXPECT_TRUE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(panther_version, 1.0));
+  EXPECT_TRUE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(panther_version, 1.06));
+  EXPECT_FALSE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(
+    panther_version, std::numeric_limits<float>::quiet_NaN()));
+  EXPECT_FALSE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(panther_version, 1.2));
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);