From fe189cd692a3ea6ca8bb9f52e553504252edae32 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 4 Dec 2020 10:19:25 +0100 Subject: [PATCH] Test Interface::updatePriority() --- core/test/test_interface_state.cpp | 51 +++++++++++++++++++++++++++++- 1 file changed, 50 insertions(+), 1 deletion(-) diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index ce0dcba5..60a71f02 100644 --- a/core/test/test_interface_state.cpp +++ b/core/test/test_interface_state.cpp @@ -1,6 +1,13 @@ -#include #include +#include +#include + +#include +#include +#include +#include #include +#include using namespace moveit::task_constructor; TEST(InterfaceStatePriority, compare) { @@ -17,3 +24,45 @@ TEST(InterfaceStatePriority, compare) { EXPECT_TRUE(Prio(0, 0) < Prio(0, inf)); EXPECT_TRUE(Prio(0, inf) > Prio(0, 0)); } + +moveit::core::RobotModelConstPtr getModel() { + ros::console::set_logger_level("ros.moveit_core.robot_model", ros::console::levels::Error); + moveit::core::RobotModelBuilder builder("robot", "base"); + builder.addChain("base->tip", "continuous"); + return builder.build(); +} + +using Prio = InterfaceState::Priority; + +// Interface that also stores passed states +class StoringInterface : public Interface +{ + std::vector> storage_; + +public: + using Interface::Interface; + void add(InterfaceState&& state) { + storage_.emplace_back(std::make_unique(std::move(state))); + Interface::add(*storage_.back()); + } + std::vector depths() const { + std::vector result; + std::transform(cbegin(), cend(), std::back_inserter(result), + [](const InterfaceState* state) { return state->priority().depth(); }); + return result; + } +}; + +TEST(Interface, update) { + auto ps = std::make_shared(getModel()); + StoringInterface i; + i.add(InterfaceState(ps, Prio(1, 0.0))); + i.add(InterfaceState(ps, Prio(3, 0.0))); + EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 1 })); + + i.updatePriority(*i.rbegin(), Prio(5, 0.0)); + EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); + + i.updatePriority(*i.begin(), Prio(1, 0.0)); + EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 1 })); +} \ No newline at end of file