diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 181b7256..9ff92a58 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -20,4 +20,7 @@ if (CATKIN_ENABLE_TESTING) catkin_add_gtest(${PROJECT_NAME}-test-cost_queue test_cost_queue.cpp) target_link_libraries(${PROJECT_NAME}-test-cost_queue ${PROJECT_NAME} gtest_main) + + catkin_add_gtest(${PROJECT_NAME}-test-interface_state test_interface_state.cpp) + target_link_libraries(${PROJECT_NAME}-test-interface_state ${PROJECT_NAME} gtest_main) endif() diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp new file mode 100644 index 00000000..a5a9d05c --- /dev/null +++ b/core/test/test_interface_state.cpp @@ -0,0 +1,19 @@ +#include +#include +#include + +using namespace moveit::task_constructor; +TEST(InterfaceStatePriority, compare) { + typedef InterfaceState::Priority Prio; + const double inf = std::numeric_limits::infinity(); + + EXPECT_TRUE(Prio(0, 0) == Prio(0, 0)); + EXPECT_TRUE(Prio(0, inf) == Prio(0, inf)); + + EXPECT_TRUE(Prio(1, 0) < Prio(0, 0)); // higher depth is smaller + EXPECT_TRUE(Prio(1, inf) < Prio(0, inf)); + + EXPECT_TRUE(Prio(0, 0) < Prio(0, 1)); // higher cost is larger + EXPECT_TRUE(Prio(0, 0) < Prio(0, inf)); + EXPECT_TRUE(Prio(0, inf) > Prio(0, 0)); +}