mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
extended unittest for serial connection validation
This commit is contained in:
parent
a4feb705d0
commit
ec2b06292e
@ -6,11 +6,14 @@
|
||||
if (CATKIN_ENABLE_TESTING)
|
||||
find_package(rostest REQUIRED)
|
||||
|
||||
add_library(gtest_utils gtest_value_printers.cpp)
|
||||
target_link_libraries(gtest_utils ${PROJECT_NAME})
|
||||
|
||||
add_rostest_gtest(${PROJECT_NAME}-test-stage test_stage.launch test_stage.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}-test-stage ${PROJECT_NAME} gtest_main)
|
||||
|
||||
catkin_add_gtest(${PROJECT_NAME}-test-container test_container.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} gtest_main)
|
||||
target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} gtest_utils gtest_main)
|
||||
|
||||
catkin_add_gtest(${PROJECT_NAME}-test-properties test_properties.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}-test-properties ${PROJECT_NAME} gtest_main)
|
||||
|
25
core/test/gtest_value_printers.cpp
Normal file
25
core/test/gtest_value_printers.cpp
Normal file
@ -0,0 +1,25 @@
|
||||
#include "gtest_value_printers.h"
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
::std::ostream& operator<<(::std::ostream& os, const InterfaceFlag& flag) {
|
||||
switch (flag) {
|
||||
case READS_START: return os << "READS_START";
|
||||
case READS_END: return os << "READS_END";
|
||||
case WRITES_NEXT_START: return os << "WRITES_NEXT_START";
|
||||
case WRITES_PREV_END: return os << "WRITES_PREV_END";
|
||||
default: return os << "unknown";
|
||||
}
|
||||
}
|
||||
|
||||
::std::ostream& operator<<(::std::ostream& os, const InterfaceFlags& flags) {
|
||||
os << "InterfaceFlags(";
|
||||
bool have_previous = false;
|
||||
for (InterfaceFlag f : {READS_START,READS_END,WRITES_NEXT_START,WRITES_PREV_END}) {
|
||||
if (flags & f) {
|
||||
os << (have_previous ? ", " : "{") << f;
|
||||
have_previous = true;
|
||||
}
|
||||
}
|
||||
return os << (have_previous ? "})" : ")");
|
||||
}
|
5
core/test/gtest_value_printers.h
Normal file
5
core/test/gtest_value_printers.h
Normal file
@ -0,0 +1,5 @@
|
||||
#include <ostream>
|
||||
#include <moveit/task_constructor/stage_p.h>
|
||||
|
||||
::std::ostream& operator<<(::std::ostream& os, const moveit::task_constructor::InterfaceFlag& flag);
|
||||
::std::ostream& operator<<(::std::ostream& os, const moveit::task_constructor::InterfaceFlags& flags);
|
@ -1,29 +1,93 @@
|
||||
#include <moveit/task_constructor/container_p.h>
|
||||
#include <moveit/task_constructor/stage_p.h>
|
||||
|
||||
#include "gtest_value_printers.h"
|
||||
#include <gtest/gtest.h>
|
||||
#include <initializer_list>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
class TestGenerator : public Generator {
|
||||
class GeneratorMockup : public Generator {
|
||||
int runs = 0;
|
||||
public:
|
||||
TestGenerator() : Generator("generator") {}
|
||||
bool canCompute() const override { return true; }
|
||||
bool compute() override { return false; }
|
||||
GeneratorMockup(int runs = 0) : Generator("generator"), runs(runs) {}
|
||||
bool canCompute() const override { return runs > 0; }
|
||||
bool compute() override { return --runs >= 0 ? true : false; }
|
||||
};
|
||||
|
||||
class TestPropagatingForward : public PropagatingForward {
|
||||
class PropagatorMockup : public PropagatingEitherWay {
|
||||
int fw_runs = 0;
|
||||
int bw_runs = 0;
|
||||
public:
|
||||
TestPropagatingForward() : PropagatingForward("forwarder") {}
|
||||
bool computeForward(const InterfaceState &from) override { return false; }
|
||||
PropagatorMockup(int fw = 0, int bw = 0) : PropagatingEitherWay("either way"), fw_runs(fw), bw_runs(bw) {}
|
||||
bool computeForward(const InterfaceState &from) override { return --fw_runs >= 0 ? true : false; }
|
||||
bool computeBackward(const InterfaceState &from) override { return --bw_runs >= 0 ? true : false; }
|
||||
};
|
||||
class ForwardMockup : public PropagatorMockup {
|
||||
public:
|
||||
ForwardMockup(int runs = 0) : PropagatorMockup(runs, 0) {
|
||||
restrictDirection(FORWARD);
|
||||
setName("forward");
|
||||
}
|
||||
};
|
||||
class BackwardMockup : public PropagatorMockup {
|
||||
public:
|
||||
BackwardMockup(int runs = 0) : PropagatorMockup(0, runs) {
|
||||
restrictDirection(BACKWARD);
|
||||
setName("backward");
|
||||
}
|
||||
};
|
||||
|
||||
class ConnectMockup : public Connecting {
|
||||
int runs = 0;
|
||||
public:
|
||||
ConnectMockup(int runs = 0) : Connecting("connect"), runs(runs) {}
|
||||
bool compute(const InterfaceState& from, const InterfaceState& to) override { return --runs >= 0 ? true : false; }
|
||||
};
|
||||
|
||||
|
||||
class BaseTest : public ::testing::Test {
|
||||
enum StageType { GEN, FW, BW, ANY, BOTH, CONN };
|
||||
void append(ContainerBase& c, const std::initializer_list<StageType>& types, int runs = 0) {
|
||||
for (StageType t : types) {
|
||||
switch (t) {
|
||||
case GEN: c.insert(std::make_unique<GeneratorMockup>(runs)); break;
|
||||
case FW: c.insert(std::make_unique<ForwardMockup>(runs)); break;
|
||||
case BW: c.insert(std::make_unique<BackwardMockup>(runs)); break;
|
||||
case ANY: c.insert(std::make_unique<PropagatorMockup>(runs, runs)); break;
|
||||
case BOTH: {
|
||||
auto s = std::make_unique<PropagatorMockup>(runs, runs);
|
||||
s->restrictDirection(PropagatingEitherWay::BOTHWAY);
|
||||
c.insert(std::move(s));
|
||||
break;
|
||||
}
|
||||
case CONN: c.insert(std::make_unique<ConnectMockup>(runs)); break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
class SerialTest : public ::testing::Test {
|
||||
protected:
|
||||
void SetUp() {}
|
||||
void TearDown() {}
|
||||
planning_scene::PlanningScenePtr scene;
|
||||
SerialContainer serial;
|
||||
InterfacePtr dummy;
|
||||
|
||||
SerialTest() : ::testing::Test(), dummy(new Interface) {}
|
||||
|
||||
void pushInterface(bool start=true, bool end=true) {
|
||||
// pretend, that the container is connected
|
||||
SerialContainerPrivate *impl = serial.pimpl();
|
||||
if (start) impl->setPrevEnds(dummy);
|
||||
if (end) impl->setNextStarts(dummy);
|
||||
}
|
||||
void reset(bool start=true, bool end=true) {
|
||||
serial.reset();
|
||||
serial.clear();
|
||||
SerialContainerPrivate *impl = serial.pimpl();
|
||||
impl->setNextStarts(InterfacePtr());
|
||||
impl->setPrevEnds(InterfacePtr());
|
||||
pushInterface(start, end);
|
||||
}
|
||||
|
||||
void validateOrder(const SerialContainerPrivate* container, const std::initializer_list<StagePrivate*> &expected) {
|
||||
size_t num = container->children().size();
|
||||
@ -44,50 +108,145 @@ protected:
|
||||
EXPECT_EQ(it, container->position(pos-num-1)) << "bad backward position resolution";
|
||||
}
|
||||
}
|
||||
void validateInit(bool start, bool end, const std::initializer_list<StageType>& types, bool expect_failure) {
|
||||
reset(start, end);
|
||||
append(serial, types);
|
||||
try {
|
||||
serial.init(scene);
|
||||
if (!expect_failure) return; // as expected
|
||||
ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << serial;
|
||||
} catch (const InitStageException &e) {
|
||||
if (expect_failure) return; // as expected
|
||||
ADD_FAILURE() << "unexpected init failure: \n" << e << "\n" << serial;
|
||||
} catch (const std::exception &e) {
|
||||
ADD_FAILURE() << "unexpected exception thrown:\n" << e.what();
|
||||
} catch (...) {
|
||||
ADD_FAILURE() << "unexpected unknown exception thrown";
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#define VALIDATE(...) {\
|
||||
SCOPED_TRACE("validateOrder({" #__VA_ARGS__ "})"); \
|
||||
validateOrder(cp, {__VA_ARGS__}); \
|
||||
validateOrder(impl, {__VA_ARGS__}); \
|
||||
}
|
||||
|
||||
TEST_F(BaseTest, serialContainer) {
|
||||
SerialContainer c("serial");
|
||||
SerialContainerPrivate *cp = c.pimpl();
|
||||
// pretend, that the container is connected
|
||||
InterfacePtr dummy(new Interface);
|
||||
cp->setNextStarts(dummy);
|
||||
cp->setPrevEnds(dummy);
|
||||
planning_scene::PlanningScenePtr scene;
|
||||
TEST_F(SerialTest, insertion_order) {
|
||||
SerialContainerPrivate *impl = serial.pimpl();
|
||||
|
||||
EXPECT_EQ(cp->parent(), nullptr);
|
||||
EXPECT_THROW(c.init(scene), InitStageException);
|
||||
EXPECT_EQ(impl->parent(), nullptr);
|
||||
EXPECT_THROW(serial.init(scene), InitStageException);
|
||||
VALIDATE();
|
||||
|
||||
/***** inserting first stage *****/
|
||||
auto g = std::make_unique<TestGenerator>();
|
||||
GeneratorPrivate *gp = g->pimpl();
|
||||
ASSERT_TRUE(c.insert(std::move(g)));
|
||||
auto g = std::make_unique<GeneratorMockup>();
|
||||
StagePrivate *gp = g->pimpl();
|
||||
ASSERT_TRUE(serial.insert(std::move(g)));
|
||||
EXPECT_FALSE(g); // ownership transferred to container
|
||||
VALIDATE(gp);
|
||||
|
||||
/***** inserting second stage *****/
|
||||
auto f = std::make_unique<TestPropagatingForward>();
|
||||
PropagatingForwardPrivate *fp = f->pimpl();
|
||||
ASSERT_TRUE(c.insert(std::move(f)));
|
||||
auto f = std::make_unique<ForwardMockup>();
|
||||
StagePrivate *fp = f->pimpl();
|
||||
ASSERT_TRUE(serial.insert(std::move(f)));
|
||||
EXPECT_FALSE(f); // ownership transferred to container
|
||||
VALIDATE(gp, fp);
|
||||
|
||||
/***** inserting third stage *****/
|
||||
auto f2 = std::make_unique<TestPropagatingForward>();
|
||||
PropagatingForwardPrivate *fp2 = f2->pimpl();
|
||||
ASSERT_TRUE(c.insert(std::move(f2), 1));
|
||||
auto f2 = std::make_unique<ForwardMockup>();
|
||||
StagePrivate *fp2 = f2->pimpl();
|
||||
ASSERT_TRUE(serial.insert(std::move(f2), 1));
|
||||
EXPECT_FALSE(f2); // ownership transferred to container
|
||||
VALIDATE(gp, fp2, fp);
|
||||
|
||||
EXPECT_NO_THROW(c.init(scene));
|
||||
|
||||
/***** inserting another generator stage *****/
|
||||
ASSERT_TRUE(c.insert(std::make_unique<TestGenerator>()));
|
||||
EXPECT_THROW(c.init(scene), InitStageException);
|
||||
auto g2 = std::make_unique<GeneratorMockup>();
|
||||
StagePrivate *gp2 = g2->pimpl();
|
||||
ASSERT_TRUE(serial.insert(std::move(g2)));
|
||||
VALIDATE(gp, fp2, fp, gp2);
|
||||
}
|
||||
|
||||
|
||||
#define EXPECT_INIT_FAILURE(start, end, ...) {\
|
||||
SCOPED_TRACE("validateInit({" #__VA_ARGS__ "})"); \
|
||||
validateInit(start, end, {__VA_ARGS__}, true); \
|
||||
}
|
||||
|
||||
#define EXPECT_INIT_SUCCESS(start, end, ...) {\
|
||||
SCOPED_TRACE("validateInit({" #__VA_ARGS__ "})"); \
|
||||
validateInit(start, end, {__VA_ARGS__}, false); \
|
||||
}
|
||||
|
||||
TEST_F(SerialTest, init_empty) {
|
||||
EXPECT_INIT_FAILURE(true, true); // no children
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({WRITES_NEXT_START, WRITES_PREV_END}));
|
||||
|
||||
EXPECT_INIT_FAILURE(false, false); // no children
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags());
|
||||
}
|
||||
|
||||
TEST_F(SerialTest, init_connecting) {
|
||||
EXPECT_INIT_SUCCESS(false, false, CONN);
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, READS_END}));
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags());
|
||||
|
||||
EXPECT_INIT_FAILURE(true, true, CONN);
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, READS_END, WRITES_NEXT_START, WRITES_PREV_END}));
|
||||
|
||||
EXPECT_INIT_FAILURE(false, false, CONN, CONN); // two connecting stages cannot be connected
|
||||
}
|
||||
|
||||
TEST_F(SerialTest, init_generator) {
|
||||
EXPECT_INIT_SUCCESS(true, true, GEN);
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({WRITES_NEXT_START, WRITES_PREV_END}));
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags());
|
||||
|
||||
EXPECT_INIT_FAILURE(false, false, GEN); // generator wants to push, but container cannot propagate pushes
|
||||
|
||||
EXPECT_INIT_FAILURE(true, true, GEN, GEN); // two generator stages cannot be connected
|
||||
}
|
||||
|
||||
TEST_F(SerialTest, init_forward) {
|
||||
// container interface doesn't match children
|
||||
EXPECT_INIT_FAILURE(false, false, FW);
|
||||
EXPECT_INIT_FAILURE(true, true, FW);
|
||||
EXPECT_INIT_FAILURE(true, false, FW);
|
||||
|
||||
// these should be fine
|
||||
EXPECT_INIT_SUCCESS(false, true, FW);
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags());
|
||||
|
||||
EXPECT_INIT_SUCCESS(false, true, FW, FW);
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags());
|
||||
|
||||
EXPECT_INIT_SUCCESS(true, true, GEN, FW);
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({WRITES_NEXT_START, WRITES_PREV_END}));
|
||||
}
|
||||
|
||||
TEST_F(SerialTest, init_backward) {
|
||||
// container interface doesn't match children
|
||||
EXPECT_INIT_FAILURE(false, false, BW);
|
||||
EXPECT_INIT_FAILURE(true, true, BW);
|
||||
EXPECT_INIT_FAILURE(false, true, BW);
|
||||
|
||||
// these should be fine
|
||||
EXPECT_INIT_SUCCESS(true, false, BW);
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags());
|
||||
|
||||
EXPECT_INIT_SUCCESS(true, false, BW, BW);
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags());
|
||||
|
||||
EXPECT_INIT_SUCCESS(true, true, BW, GEN);
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({WRITES_NEXT_START, WRITES_PREV_END}));
|
||||
|
||||
EXPECT_INIT_SUCCESS(true, true, BW, GEN, FW);
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({WRITES_NEXT_START, WRITES_PREV_END}));
|
||||
}
|
||||
|
||||
TEST_F(SerialTest, init_auto) {
|
||||
// derive propagation direction from outer interface
|
||||
EXPECT_INIT_SUCCESS(false, true, ANY); // -> ->
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, WRITES_NEXT_START}));
|
||||
EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // <- <-
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, WRITES_NEXT_START}));
|
||||
}
|
||||
|
@ -8,13 +8,13 @@
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
class TestGenerator : public Generator {
|
||||
class GeneratorMockup : public Generator {
|
||||
planning_scene::PlanningScenePtr ps;
|
||||
InterfacePtr prev;
|
||||
InterfacePtr next;
|
||||
|
||||
public:
|
||||
TestGenerator() : Generator("generator") {
|
||||
GeneratorMockup() : Generator("generator") {
|
||||
robot_model_loader::RobotModelLoader loader;
|
||||
ps.reset(new planning_scene::PlanningScene(loader.getModel()));
|
||||
|
||||
@ -35,7 +35,7 @@ TEST(Stage, registerCallbacks) {
|
||||
char *argv = nullptr;
|
||||
ros::init(argc, &argv, "testLocalTaskModel");
|
||||
|
||||
TestGenerator g;
|
||||
GeneratorMockup g;
|
||||
uint called = 0;
|
||||
auto cb = [&called](const SolutionBase &s) {
|
||||
++called;
|
||||
|
Loading…
Reference in New Issue
Block a user