diff --git a/husarion_ugv_manager/CMakeLists.txt b/husarion_ugv_manager/CMakeLists.txt index a7c0dbf91..952a1e227 100644 --- a/husarion_ugv_manager/CMakeLists.txt +++ b/husarion_ugv_manager/CMakeLists.txt @@ -85,11 +85,13 @@ add_executable(safety_manager_node src/safety_manager_node_main.cpp ament_target_dependencies( safety_manager_node behaviortree_ros2 - panther_msgs + geometry_msgs husarion_ugv_utils + panther_msgs rclcpp sensor_msgs - std_msgs) + std_msgs + tf2_geometry_msgs) generate_parameter_library(safety_manager_parameters config/safety_manager_parameters.yaml) @@ -101,11 +103,13 @@ add_executable(lights_manager_node src/lights_manager_node_main.cpp ament_target_dependencies( lights_manager_node behaviortree_ros2 - panther_msgs + geometry_msgs husarion_ugv_utils + panther_msgs rclcpp sensor_msgs - std_msgs) + std_msgs + tf2_geometry_msgs) generate_parameter_library(lights_manager_parameters config/lights_manager_parameters.yaml) @@ -222,7 +226,7 @@ if(BUILD_TESTING) $) 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) @@ -243,11 +247,13 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_lights_manager_node behaviortree_cpp behaviortree_ros2 - panther_msgs + geometry_msgs husarion_ugv_utils + panther_msgs rclcpp sensor_msgs - std_msgs) + std_msgs + tf2_geometry_msgs) target_link_libraries(${PROJECT_NAME}_test_lights_manager_node lights_manager_parameters) @@ -262,11 +268,13 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_lights_behavior_tree behaviortree_cpp behaviortree_ros2 - panther_msgs + geometry_msgs husarion_ugv_utils + panther_msgs rclcpp sensor_msgs - std_msgs) + std_msgs + tf2_geometry_msgs) target_link_libraries(${PROJECT_NAME}_test_lights_behavior_tree lights_manager_parameters) @@ -280,11 +288,13 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_safety_manager_node behaviortree_cpp behaviortree_ros2 - panther_msgs + geometry_msgs husarion_ugv_utils + panther_msgs rclcpp sensor_msgs - std_msgs) + std_msgs + tf2_geometry_msgs) target_link_libraries(${PROJECT_NAME}_test_safety_manager_node safety_manager_parameters) @@ -299,12 +309,14 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_safety_behavior_tree behaviortree_cpp behaviortree_ros2 - panther_msgs + geometry_msgs husarion_ugv_utils + panther_msgs 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_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/plugins/action/dock_robot_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/dock_robot_node.hpp index 7793bd216..a050b947e 100644 --- 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 @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_ -#define PANTHER_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_ +#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_ +#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_ #include #include @@ -24,7 +24,7 @@ #include #include -namespace panther_manager +namespace husarion_ugv_manager { class DockRobot : public BT::RosActionNode @@ -76,6 +76,6 @@ class DockRobot : public BT::RosActionNode #include @@ -24,7 +24,7 @@ #include #include -namespace panther_manager +namespace husarion_ugv_manager { class UndockRobot : public BT::RosActionNode @@ -61,6 +61,6 @@ class UndockRobot : public BT::RosActionNode -#include "panther_manager/behavior_tree_utils.hpp" +#include "husarion_ugv_manager/behavior_tree_utils.hpp" -namespace panther_manager +namespace husarion_ugv_manager { // FIXME: There is no possibility to set QoS profile. Add it in the future to subscribe e_stop. @@ -50,6 +50,6 @@ class CheckBoolMsg : public BT::RosTopicSubNode } }; -} // namespace panther_manager +} // namespace husarion_ugv_manager #endif // PANTHER_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 index a7966482b..d2cf6e72b 100644 --- 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 @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_ -#define PANTHER_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_ +#ifndef HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_ +#define HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_ #include #include @@ -24,9 +24,9 @@ #include -#include "panther_manager/behavior_tree_utils.hpp" +#include "husarion_ugv_manager/behavior_tree_utils.hpp" -namespace panther_manager +namespace husarion_ugv_manager { class CheckJoyMsg : public BT::RosTopicSubNode @@ -63,6 +63,6 @@ class CheckJoyMsg : public BT::RosTopicSubNode bool checkTimeout(const JoyMsg::SharedPtr & last_msg); }; -} // namespace panther_manager +} // namespace husarion_ugv_manager -#endif // PANTHER_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_ +#endif // HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_ diff --git a/husarion_ugv_manager/src/plugins/action/dock_robot_node.cpp b/husarion_ugv_manager/src/plugins/action/dock_robot_node.cpp index 8f5a60cf0..b7e3dd3c5 100644 --- a/husarion_ugv_manager/src/plugins/action/dock_robot_node.cpp +++ b/husarion_ugv_manager/src/plugins/action/dock_robot_node.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_manager/plugins/action/dock_robot_node.hpp" -#include "panther_manager/behavior_tree_utils.hpp" +#include "husarion_ugv_manager/plugins/action/dock_robot_node.hpp" +#include "husarion_ugv_manager/behavior_tree_utils.hpp" -namespace panther_manager +namespace husarion_ugv_manager { bool DockRobot::setGoal(Goal & goal) @@ -83,7 +83,7 @@ void DockRobot::onHalt() RCLCPP_INFO_STREAM(this->logger(), GetLoggerPrefix(name()) << ": onHalt"); } -} // namespace panther_manager +} // namespace husarion_ugv_manager #include "behaviortree_ros2/plugins.hpp" -CreateRosNodePlugin(panther_manager::DockRobot, "DockRobot"); +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 index e47c000a9..bee50f39b 100644 --- a/husarion_ugv_manager/src/plugins/action/undock_robot_node.cpp +++ b/husarion_ugv_manager/src/plugins/action/undock_robot_node.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_manager/plugins/action/undock_robot_node.hpp" -#include "panther_manager/behavior_tree_utils.hpp" +#include "husarion_ugv_manager/plugins/action/undock_robot_node.hpp" +#include "husarion_ugv_manager/behavior_tree_utils.hpp" -namespace panther_manager +namespace husarion_ugv_manager { bool UndockRobot::setGoal(Goal & goal) @@ -60,7 +60,7 @@ void UndockRobot::onHalt() RCLCPP_INFO_STREAM(this->logger(), GetLoggerPrefix(name()) << ": onHalt"); } -} // namespace panther_manager +} // namespace husarion_ugv_manager #include "behaviortree_ros2/plugins.hpp" -CreateRosNodePlugin(panther_manager::UndockRobot, "UndockRobot"); +CreateRosNodePlugin(husarion_ugv_manager::UndockRobot, "UndockRobot"); diff --git a/husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp b/husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp index 09cd7f0d1..892d03810 100644 --- a/husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp +++ b/husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_manager/plugins/condition/check_bool_msg.hpp" +#include "husarion_ugv_manager/plugins/condition/check_bool_msg.hpp" -#include "panther_manager/behavior_tree_utils.hpp" +#include "husarion_ugv_manager/behavior_tree_utils.hpp" -namespace panther_manager +namespace husarion_ugv_manager { BT::NodeStatus CheckBoolMsg::onTick(const BoolMsg::SharedPtr & last_msg) @@ -28,7 +28,7 @@ BT::NodeStatus CheckBoolMsg::onTick(const BoolMsg::SharedPtr & last_msg) : BT::NodeStatus::FAILURE; } -} // namespace panther_manager +} // namespace husarion_ugv_manager #include "behaviortree_ros2/plugins.hpp" -CreateRosNodePlugin(panther_manager::CheckBoolMsg, "CheckBoolMsg"); +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 index 49884c3fc..8d28cbe1e 100644 --- a/husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp +++ b/husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_manager/plugins/condition/check_joy_msg.hpp" +#include "husarion_ugv_manager/plugins/condition/check_joy_msg.hpp" -#include "panther_manager/behavior_tree_utils.hpp" +#include "husarion_ugv_manager/behavior_tree_utils.hpp" -namespace panther_manager +namespace husarion_ugv_manager { BT::NodeStatus CheckJoyMsg::onTick(const JoyMsg::SharedPtr & last_msg) @@ -86,7 +86,7 @@ bool CheckJoyMsg::checkTimeout(const JoyMsg::SharedPtr & last_msg) return (max_timeout <= 0.0) || time_diff < rclcpp::Duration::from_seconds(max_timeout); } -} // namespace panther_manager +} // namespace husarion_ugv_manager #include "behaviortree_ros2/plugins.hpp" -CreateRosNodePlugin(panther_manager::CheckJoyMsg, "CheckJoyMsg"); +CreateRosNodePlugin(husarion_ugv_manager::CheckJoyMsg, "CheckJoyMsg"); 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 index ca45c50c9..97f66cecc 100644 --- a/husarion_ugv_manager/test/plugins/action/test_dock_robot_node.cpp +++ b/husarion_ugv_manager/test/plugins/action/test_dock_robot_node.cpp @@ -22,10 +22,10 @@ #include -#include "panther_manager/plugins/action/dock_robot_node.hpp" +#include "husarion_ugv_manager/plugins/action/dock_robot_node.hpp" #include "utils/plugin_test_utils.hpp" -class TestDockRobot : public panther_manager::plugin_test_utils::PluginTestUtils +class TestDockRobot : public husarion_ugv_manager::plugin_test_utils::PluginTestUtils { public: using Action = opennav_docking_msgs::action::DockRobot; @@ -72,7 +72,7 @@ TEST_F(TestDockRobot, GoodLoadingDockRobotPlugin) {"navigate_to_staging_pose", "false"}, }; - RegisterNodeWithParams("DockRobot"); + RegisterNodeWithParams("DockRobot"); ASSERT_NO_THROW({ CreateTree("DockRobot", params); }); } @@ -85,7 +85,7 @@ TEST_F(TestDockRobot, WrongLoadingDockRobotPlugin) {"max_staging_time", "5.0"}, {"navigate_to_staging_pose", "false"}, }; - RegisterNodeWithParams("DockRobot"); + RegisterNodeWithParams("DockRobot"); EXPECT_THROW({ CreateTree("WrongDockRobot", params); }, BT::RuntimeError); } @@ -100,7 +100,7 @@ TEST_F(TestDockRobot, WrongCallDockRobotServerNotInitialized) {"max_staging_time", "5.0"}, {"navigate_to_staging_pose", "false"}, }; - RegisterNodeWithParams("DockRobot"); + RegisterNodeWithParams("DockRobot"); CreateTree("DockRobot", params); @@ -121,7 +121,7 @@ TEST_F(TestDockRobot, WrongCallDockRobotServerWithNoDockID) {"navigate_to_staging_pose", "false"}, }; - RegisterNodeWithParams("DockRobot"); + RegisterNodeWithParams("DockRobot"); CreateTree("DockRobot", params); auto & tree = GetTree(); @@ -140,7 +140,7 @@ TEST_F(TestDockRobot, CallDockRobotServerWithoutDockID) {"navigate_to_staging_pose", "false"}, }; - RegisterNodeWithParams("DockRobot"); + RegisterNodeWithParams("DockRobot"); CreateTree("DockRobot", params); auto & tree = GetTree(); @@ -162,7 +162,7 @@ TEST_F(TestDockRobot, CallDockRobotServerWithEmptyDockID) {"navigate_to_staging_pose", "true"}, }; - RegisterNodeWithParams("DockRobot"); + RegisterNodeWithParams("DockRobot"); CreateTree("DockRobot", params); auto & tree = GetTree(); @@ -184,7 +184,7 @@ TEST_F(TestDockRobot, CallDockRobotServerWithEmptyDockType) {"navigate_to_staging_pose", "true"}, }; - RegisterNodeWithParams("DockRobot"); + RegisterNodeWithParams("DockRobot"); CreateTree("DockRobot", params); auto & tree = GetTree(); @@ -205,7 +205,7 @@ TEST_F(TestDockRobot, CallDockRobotServerWithNavigateToStagingPoseFailure) {"navigate_to_staging_pose", "true"}, }; - RegisterNodeWithParams("DockRobot"); + RegisterNodeWithParams("DockRobot"); CreateTree("DockRobot", params); auto & tree = GetTree(); @@ -227,7 +227,7 @@ TEST_F(TestDockRobot, CallDockRobotServerWithNavigateToStagingPoseSuccess) {"navigate_to_staging_pose", "true"}, }; - RegisterNodeWithParams("DockRobot"); + RegisterNodeWithParams("DockRobot"); CreateTree("DockRobot", params); auto & tree = GetTree(); 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 index 5820bfded..ea8f53692 100644 --- a/husarion_ugv_manager/test/plugins/action/test_undock_robot_node.cpp +++ b/husarion_ugv_manager/test/plugins/action/test_undock_robot_node.cpp @@ -22,10 +22,10 @@ #include -#include "panther_manager/plugins/action/undock_robot_node.hpp" +#include "husarion_ugv_manager/plugins/action/undock_robot_node.hpp" #include "utils/plugin_test_utils.hpp" -class TestUndockRobot : public panther_manager::plugin_test_utils::PluginTestUtils +class TestUndockRobot : public husarion_ugv_manager::plugin_test_utils::PluginTestUtils { public: using Action = opennav_docking_msgs::action::UndockRobot; @@ -71,7 +71,7 @@ TEST_F(TestUndockRobot, GoodLoadingUndockRobotPlugin) {"max_undocking_time", "5.0"}, }; - RegisterNodeWithParams("UndockRobot"); + RegisterNodeWithParams("UndockRobot"); ASSERT_NO_THROW({ CreateTree("UndockRobot", params); }); } @@ -84,7 +84,7 @@ TEST_F(TestUndockRobot, WrongLoadingUndockRobotPlugin) {"max_undocking_time", "5.0"}, }; - RegisterNodeWithParams("UndockRobot"); + RegisterNodeWithParams("UndockRobot"); EXPECT_THROW({ CreateTree("WrongUndockRobot", params); }, BT::RuntimeError); } @@ -97,7 +97,7 @@ TEST_F(TestUndockRobot, WrongCallUndockRobotServerNotInitialized) {"max_undocking_time", "5.0"}, }; - RegisterNodeWithParams("UndockRobot"); + RegisterNodeWithParams("UndockRobot"); CreateTree("UndockRobot", params); @@ -118,7 +118,7 @@ TEST_F(TestUndockRobot, WrongCallUndockRobotServerWithNoDockType) {"max_undocking_time", "5.0"}, }; - RegisterNodeWithParams("UndockRobot"); + RegisterNodeWithParams("UndockRobot"); CreateTree("UndockRobot", params); auto & tree = GetTree(); @@ -136,7 +136,7 @@ TEST_F(TestUndockRobot, CallUndockRobotServerFailure) {"max_undocking_time", "5.0"}, }; - RegisterNodeWithParams("UndockRobot"); + RegisterNodeWithParams("UndockRobot"); CreateTree("UndockRobot", params); auto & tree = GetTree(); @@ -155,7 +155,7 @@ TEST_F(TestUndockRobot, CallUndockRobotServerSuccess) {"max_undocking_time", "5.0"}, }; - RegisterNodeWithParams("UndockRobot"); + RegisterNodeWithParams("UndockRobot"); CreateTree("UndockRobot", params); auto & tree = GetTree(); 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 +#include +#include +#include +#include + +#include + +#include +#include + +#include + +#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; + +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::SharedPtr publisher_; +}; + +TestCheckBoolMsg::TestCheckBoolMsg() +{ + RegisterNodeWithParams(PLUGIN); + publisher_ = bt_node_->create_publisher(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 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 +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include + +#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; + +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 & axes = {}, const std::vector & buttons = {}, + const HeaderMsg & header = HeaderMsg()); + void SetCurrentMsgTime(JoyMsg & header); + +protected: + rclcpp::Publisher::SharedPtr joy_publisher_; +}; + +TestCheckJoyMsg::TestCheckJoyMsg() +{ + RegisterNodeWithParams(PLUGIN); + joy_publisher_ = bt_node_->create_publisher(TOPIC, 10); +} + +JoyMsg TestCheckJoyMsg::CreateMsg( + const std::vector & axes, const std::vector & 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 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 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_utils/include/husarion_ugv_utils/tf2_utils.hpp b/husarion_ugv_utils/include/husarion_ugv_utils/tf2_utils.hpp index 54e5547ea..f954aa962 100644 --- a/husarion_ugv_utils/include/husarion_ugv_utils/tf2_utils.hpp +++ b/husarion_ugv_utils/include/husarion_ugv_utils/tf2_utils.hpp @@ -23,7 +23,7 @@ #include #include -namespace panther_utils::tf2_utils +namespace husarion_ugv_utils::tf2_utils { /** @@ -137,6 +137,6 @@ bool ArePosesNear( std::abs(pose_1_rpy.y - pose_2_rpy.y) < angle_tolerance && std::abs(pose_1_rpy.z - pose_2_rpy.z) < angle_tolerance; } -} // namespace panther_utils::tf2_utils +} // namespace husarion_ugv_utils::tf2_utils #endif // PANTHER_UTILS_TF2_UTILS_HPP_ diff --git a/husarion_ugv_utils/test/test_common_utilities.cpp b/husarion_ugv_utils/test/test_common_utilities.cpp index 2143fbeb7..cb189751d 100644 --- a/husarion_ugv_utils/test/test_common_utilities.cpp +++ b/husarion_ugv_utils/test/test_common_utilities.cpp @@ -199,12 +199,12 @@ TEST(TestMeetsVersionRequirement, NaNVersionRequired) TEST(MeetsVersionRequirement, CorrectlyComparesVersions) { float panther_version = 1.06; - EXPECT_TRUE(panther_utils::common_utilities::MeetsVersionRequirement(panther_version, 0.0)); - EXPECT_TRUE(panther_utils::common_utilities::MeetsVersionRequirement(panther_version, 1.0)); - EXPECT_TRUE(panther_utils::common_utilities::MeetsVersionRequirement(panther_version, 1.06)); - EXPECT_FALSE(panther_utils::common_utilities::MeetsVersionRequirement( + 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::quiet_NaN())); - EXPECT_FALSE(panther_utils::common_utilities::MeetsVersionRequirement(panther_version, 1.2)); + EXPECT_FALSE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(panther_version, 1.2)); } int main(int argc, char ** argv) diff --git a/husarion_ugv_utils/test/test_tf2_utils.cpp b/husarion_ugv_utils/test/test_tf2_utils.cpp index e2bd1f526..c23c6808e 100644 --- a/husarion_ugv_utils/test/test_tf2_utils.cpp +++ b/husarion_ugv_utils/test/test_tf2_utils.cpp @@ -25,7 +25,7 @@ #include #include -#include "panther_utils/tf2_utils.hpp" +#include "husarion_ugv_utils/tf2_utils.hpp" static constexpr char kBaseFrame[] = "base_link"; static constexpr char kOdomFrame[] = "odom"; @@ -73,7 +73,7 @@ TEST_F(TestTransformPose, EmptyFrame) { geometry_msgs::msg::PoseStamped pose; EXPECT_THROW( - { panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, ""); }, std::runtime_error); + { husarion_ugv_utils::tf2_utils::TransformPose(tf2_buffer_, pose, ""); }, std::runtime_error); } TEST_F(TestTransformPose, TransformMissing) @@ -81,11 +81,11 @@ TEST_F(TestTransformPose, TransformMissing) geometry_msgs::msg::PoseStamped pose; EXPECT_THROW( - { panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, kBaseFrame); }, + { husarion_ugv_utils::tf2_utils::TransformPose(tf2_buffer_, pose, kBaseFrame); }, std::runtime_error); pose.header.frame_id = kOdomFrame; EXPECT_THROW( - { panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, kBaseFrame); }, + { husarion_ugv_utils::tf2_utils::TransformPose(tf2_buffer_, pose, kBaseFrame); }, std::runtime_error); } @@ -97,7 +97,7 @@ TEST_F(TestTransformPose, TransformWithinSameFrame) pose.header.stamp = clock_->now(); ASSERT_NO_THROW( - { pose = panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, kOdomFrame, 10.0); };); + { pose = husarion_ugv_utils::tf2_utils::TransformPose(tf2_buffer_, pose, kOdomFrame, 10.0); };); EXPECT_NEAR(pose.pose.position.x, 0.1, 0.01); EXPECT_NEAR(pose.pose.position.y, 0.0, 0.01); EXPECT_NEAR(pose.pose.position.z, 0.0, 0.01); @@ -115,7 +115,7 @@ TEST_F(TestTransformPose, TransformToDifferentFrame) SetBaseLinkToOdomTransform(stamp); ASSERT_NO_THROW( - { pose = panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, kBaseFrame, 10.0); };); + { pose = husarion_ugv_utils::tf2_utils::TransformPose(tf2_buffer_, pose, kBaseFrame, 10.0); };); EXPECT_NEAR(pose.pose.position.x, -0.2, 0.01); EXPECT_NEAR(pose.pose.position.y, -0.2, 0.01); EXPECT_NEAR(pose.pose.position.z, -0.1, 0.01); @@ -133,7 +133,7 @@ TEST_F(TestTransformPose, TestTimeout) pose.header.stamp.sec += 5; EXPECT_THROW( - { panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, kBaseFrame, 1.0); }, + { husarion_ugv_utils::tf2_utils::TransformPose(tf2_buffer_, pose, kBaseFrame, 1.0); }, std::runtime_error); } @@ -148,7 +148,7 @@ TEST(OffsetPose, CheckDefaultPose) transform.setRotation(rotation); geometry_msgs::msg::PoseStamped pose; - auto offset_pose = panther_utils::tf2_utils::OffsetPose(pose, transform); + auto offset_pose = husarion_ugv_utils::tf2_utils::OffsetPose(pose, transform); EXPECT_NEAR(offset_pose.pose.position.x, translation.getX(), 0.01); EXPECT_NEAR(offset_pose.pose.position.y, translation.getY(), 0.01); @@ -182,7 +182,7 @@ TEST(OffsetPose, CheckDefinedPose) pose_rotation.setRPY(M_PI, M_PI / 2.0, 0.0); pose.pose.orientation = tf2::toMsg(pose_rotation); - auto offset_pose = panther_utils::tf2_utils::OffsetPose(pose, transform); + auto offset_pose = husarion_ugv_utils::tf2_utils::OffsetPose(pose, transform); EXPECT_NEAR(offset_pose.pose.position.x, pose.pose.position.x - translation.getZ(), 0.01); EXPECT_NEAR(offset_pose.pose.position.y, pose.pose.position.y - translation.getY(), 0.01); @@ -204,7 +204,7 @@ TEST(TestArePosesNear, EmptyFrame) geometry_msgs::msg::PoseStamped pose_2; EXPECT_THROW( - { panther_utils::tf2_utils::ArePosesNear(pose_1, pose_2, 0.1, 0.1); }, std::runtime_error); + { husarion_ugv_utils::tf2_utils::ArePosesNear(pose_1, pose_2, 0.1, 0.1); }, std::runtime_error); } TEST(TestArePosesNear, DifferentFrames) @@ -215,7 +215,7 @@ TEST(TestArePosesNear, DifferentFrames) pose_2.header.frame_id = kOdomFrame; EXPECT_THROW( - { panther_utils::tf2_utils::ArePosesNear(pose_1, pose_2, 0.1, 0.1); }, std::runtime_error); + { husarion_ugv_utils::tf2_utils::ArePosesNear(pose_1, pose_2, 0.1, 0.1); }, std::runtime_error); } TEST(TestArePosesNear, TooFarDistance) @@ -232,7 +232,7 @@ TEST(TestArePosesNear, TooFarDistance) pose_2.pose.position.y = 0.2; pose_2.pose.position.z = 0.0; - EXPECT_FALSE(panther_utils::tf2_utils::ArePosesNear(pose_1, pose_2, 0.05, 0.05)); + EXPECT_FALSE(husarion_ugv_utils::tf2_utils::ArePosesNear(pose_1, pose_2, 0.05, 0.05)); } TEST(TestArePosesNear, TooFarAngle) @@ -249,7 +249,7 @@ TEST(TestArePosesNear, TooFarAngle) pose_2_rotation.setRPY(0.0, 0.0, 0.2); pose_2.pose.orientation = tf2::toMsg(pose_2_rotation); - EXPECT_FALSE(panther_utils::tf2_utils::ArePosesNear(pose_1, pose_2, 0.1, 0.05)); + EXPECT_FALSE(husarion_ugv_utils::tf2_utils::ArePosesNear(pose_1, pose_2, 0.1, 0.05)); } TEST(TestArePosesNear, NearAngleAndDistance) @@ -272,7 +272,7 @@ TEST(TestArePosesNear, NearAngleAndDistance) pose_2_rotation.setRPY(0.0, 0.0, 0.1); pose_2.pose.orientation = tf2::toMsg(pose_2_rotation); - EXPECT_TRUE(panther_utils::tf2_utils::ArePosesNear(pose_1, pose_2, 0.2, 0.2)); + EXPECT_TRUE(husarion_ugv_utils::tf2_utils::ArePosesNear(pose_1, pose_2, 0.2, 0.2)); } int main(int argc, char ** argv)