diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 1af29b5a..5cb8af15 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -73,7 +73,7 @@ public: inline const container_type& children() const { return children_; } /** Retrieve iterator into children_ pointing to indexed element. - * Negative index counts from end(). + * Negative index counts from end(), i.e. -1 is end(), -2 is --end(), etc. * Contrary to std::advance(), iterator limits are considered. */ const_iterator position(int index) const; diff --git a/core/src/container.cpp b/core/src/container.cpp index f6d6f559..bb25060f 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -65,7 +65,7 @@ ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int index) c container_type::const_reverse_iterator from_end = children_.rbegin(); for (auto end = children_.rend(); index < 0 && from_end != end; ++index) ++from_end; - position = from_end.base(); + position = index < 0 ? children_.end() : from_end.base(); } return position; } diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index d72d7ec0..678e5efb 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -66,6 +66,40 @@ void append(ContainerBase& c, const std::initializer_list& types, int } +class NamedStage : public GeneratorMockup { +public: + NamedStage(const std::string& name) : GeneratorMockup() { + setName(name); + } +}; + +TEST(ContainerBase, position) { + SerialContainer s; + SerialContainerPrivate *impl = s.pimpl(); + + EXPECT_EQ(impl->position(0), impl->children().end()); + EXPECT_EQ(impl->position(1), impl->children().end()); + EXPECT_EQ(impl->position(-1), impl->children().end()); + EXPECT_EQ(impl->position(-2), impl->children().end()); + + s.insert(std::make_unique("0")); + EXPECT_STREQ((*impl->position(0))->name().c_str(), "0"); + EXPECT_EQ(impl->position(-1), impl->children().end()); + EXPECT_STREQ((*impl->position(-2))->name().c_str(), "0"); + EXPECT_EQ(impl->position(-3), impl->children().end()); + + s.insert(std::make_unique("1")); + EXPECT_STREQ((*impl->position(0))->name().c_str(), "0"); + EXPECT_STREQ((*impl->position(1))->name().c_str(), "1"); + EXPECT_EQ(impl->position(2), impl->children().end()); + + EXPECT_EQ(impl->position(-1), impl->children().end()); + EXPECT_STREQ((*impl->position(-2))->name().c_str(), "1"); + EXPECT_STREQ((*impl->position(-3))->name().c_str(), "0"); + EXPECT_EQ(impl->position(-4), impl->children().end()); +} + + class SerialTest : public ::testing::Test { protected: moveit::core::RobotModelConstPtr robot_model;