diff --git a/core/include/moveit/task_constructor/cost_queue.h b/core/include/moveit/task_constructor/cost_queue.h index 14fff8b5..a8f9d78a 100644 --- a/core/include/moveit/task_constructor/cost_queue.h +++ b/core/include/moveit/task_constructor/cost_queue.h @@ -6,6 +6,18 @@ #include #include +/// ValueOrPointeeLess provides correct comparison for plain and pointer-like types +template +struct ValueOrPointeeLess : public std::less {}; + +/// The following template-specialization is for pointer-like types +template +struct ValueOrPointeeLess(), std::declval())> { + bool operator()(const T& x, const T& y) const { + return *x < *y; + } +}; + /** * @brief ordered provides an adapter for a std::list to allow sorting. * @@ -14,7 +26,7 @@ * Sorted insertion has logarithmic complexity. */ template > + typename Compare = ValueOrPointeeLess> class ordered { public: @@ -50,7 +62,7 @@ public: reference top() { return c.front(); } const_reference top() const { return c.front(); } - reference pop() { reference result = top(); c.pop_front(); return result; } + value_type pop() { value_type result(top()); c.pop_front(); return result; } reference front() { return c.front(); } const_reference front() const { return c.front(); } @@ -81,6 +93,8 @@ public: iterator at = std::upper_bound(c.begin(), c.end(), item, comp); return c.insert(at, std::move(item)); } + inline void push(const value_type& item) { insert(item); } + inline void push(value_type&& item) { insert(std::move(item)); } iterator erase(const_iterator pos) { return c.erase(pos); } diff --git a/core/include/moveit/task_constructor/stages/compute_ik.h b/core/include/moveit/task_constructor/stages/compute_ik.h index c0b65954..9f829f25 100644 --- a/core/include/moveit/task_constructor/stages/compute_ik.h +++ b/core/include/moveit/task_constructor/stages/compute_ik.h @@ -37,6 +37,7 @@ #pragma once #include +#include #include #include @@ -66,7 +67,8 @@ class ComputeIK : public WrapperBase { public: ComputeIK(const std::string &name, Stage::pointer &&child = Stage::pointer()); - void init(const core::RobotModelConstPtr &robot_model); + void reset() override; + void init(const core::RobotModelConstPtr &robot_model) override; void onNewSolution(const SolutionBase &s) override; bool canCompute() const override; @@ -113,7 +115,7 @@ public: } protected: - std::queue targets_; + ordered upstream_solutions_; }; } } } diff --git a/core/include/moveit/task_constructor/stages/generate_pose.h b/core/include/moveit/task_constructor/stages/generate_pose.h index b0cd1dbb..81c4cace 100644 --- a/core/include/moveit/task_constructor/stages/generate_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_pose.h @@ -39,6 +39,7 @@ #pragma once #include +#include #include namespace moveit { namespace task_constructor { namespace stages { @@ -57,8 +58,7 @@ public: protected: void onNewSolution(const SolutionBase& s) override; - - std::deque scenes_; + ordered upstream_solutions_; }; } } } diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index a07fb3fb..74af502f 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -134,17 +134,9 @@ private: Interface* owner_ = nullptr; // allow update of priority }; - -/// compare InterfaceState* by value -struct InterfaceStateLess { - inline bool operator()(const InterfaceState* x, const InterfaceState* y) const { - return *x < *y; - } -}; - /** Interface provides a cost-sorted list of InterfaceStates available as input for a stage. */ -class Interface : public ordered { - typedef ordered base_type; +class Interface : public ordered { + typedef ordered base_type; public: // iterators providing convinient access to stored InterfaceState diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index be5b779f..879b4947 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -179,6 +179,12 @@ bool validateGroup(const PropertyMap& props, const moveit::core::RobotModelConst } // anonymous namespace +void ComputeIK::reset() +{ + upstream_solutions_.clear(); + WrapperBase::reset(); +} + void ComputeIK::init(const moveit::core::RobotModelConstPtr& robot_model) { InitStageException errors; @@ -204,12 +210,12 @@ void ComputeIK::onNewSolution(const SolutionBase &s) assert(s.start() && s.end()); assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator - // TODO: don't take pointers, this is dangerous - targets_.push(&s); + // It's safe to store a pointer to the solution, as the generating stage stores it + upstream_solutions_.push(&s); } bool ComputeIK::canCompute() const { - return !targets_.empty() || WrapperBase::canCompute(); + return !upstream_solutions_.empty() || WrapperBase::canCompute(); } void ComputeIK::compute() @@ -217,11 +223,10 @@ void ComputeIK::compute() if(WrapperBase::canCompute()) WrapperBase::compute(); - if(targets_.empty()) + if(upstream_solutions_.empty()) return; - const SolutionBase& s = *targets_.front(); - targets_.pop(); + const SolutionBase& s = *upstream_solutions_.pop(); // -1 TODO: this should not be necessary in my opinion: Why do you think so? // It is, because the properties on the interface might change from call to call... diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 87070483..e129005b 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -90,16 +90,9 @@ void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model) void GenerateGraspPose::onNewSolution(const SolutionBase& s) { - planning_scene::PlanningScenePtr scene = s.end()->scene()->diff(); + planning_scene::PlanningSceneConstPtr scene = s.end()->scene(); - // set end effector pose const auto& props = properties(); - const std::string& eef = props.get("eef"); - const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef); - - robot_state::RobotState &robot_state = scene->getCurrentStateNonConst(); - robot_state.setToDefaultValues(jmg , props.get("pregrasp")); - const std::string& object = props.get("object"); if (!scene->knowsFrameTransform(object)) { const std::string msg = "object '" + object + "' not in scene"; @@ -114,16 +107,21 @@ void GenerateGraspPose::onNewSolution(const SolutionBase& s) return; } - scenes_.push_back(scene); + upstream_solutions_.push(&s); } void GenerateGraspPose::compute() { - if (scenes_.empty()) + if (upstream_solutions_.empty()) return; - planning_scene::PlanningScenePtr scene = scenes_[0]; - scenes_.pop_front(); + planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff(); + // set end effector pose const auto& props = properties(); + const std::string& eef = props.get("eef"); + const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef); + + robot_state::RobotState &robot_state = scene->getCurrentStateNonConst(); + robot_state.setToDefaultValues(jmg , props.get("pregrasp")); geometry_msgs::PoseStamped target_pose_msg; target_pose_msg.header.frame_id = props.get("object"); diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp index fe8d07d2..d6d9c139 100644 --- a/core/src/stages/generate_pose.cpp +++ b/core/src/stages/generate_pose.cpp @@ -51,25 +51,25 @@ GeneratePose::GeneratePose(const std::string& name) void GeneratePose::reset() { + upstream_solutions_.clear(); MonitoringGenerator::reset(); - scenes_.clear(); } void GeneratePose::onNewSolution(const SolutionBase& s) { - scenes_.push_back(s.end()->scene()->diff()); + // It's safe to store a pointer to this solution, as the generating stage stores it + upstream_solutions_.push(&s); } bool GeneratePose::canCompute() const { - return scenes_.size() > 0; + return upstream_solutions_.size() > 0; } void GeneratePose::compute() { - if (scenes_.empty()) + if (upstream_solutions_.empty()) return; - planning_scene::PlanningScenePtr scene = scenes_[0]; - scenes_.pop_front(); + planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff(); geometry_msgs::PoseStamped target_pose = properties().get("pose"); if (target_pose.header.frame_id.empty()) target_pose.header.frame_id = scene->getPlanningFrame(); diff --git a/core/test/test_cost_queue.cpp b/core/test/test_cost_queue.cpp index 3a754709..0143f2c8 100644 --- a/core/test/test_cost_queue.cpp +++ b/core/test/test_cost_queue.cpp @@ -1,6 +1,100 @@ #include +#include #include +#include #include +#include + +namespace mtc = moveit::task_constructor; + +// type-trait functions for OrderedTest +template T create(int cost); +template <> int create(int cost) { return cost; } +template <> int* create(int cost) { return new int(cost); } +template <> mtc::SolutionBasePtr create(int cost) { + mtc::SolutionBasePtr s = std::make_shared(); + s->setCost(cost); + return s; +} +template <> mtc::SolutionBaseConstPtr create(int cost) { + return create(cost); +} + +template int value(const T& item); +template <> int value(const int& item) { return item; } +template <> int value(int* const& item) { return *item; } +template <> int value(const mtc::SolutionBasePtr& item) { return item->cost(); } +template <> int value(const mtc::SolutionBaseConstPtr& item) { return item->cost(); } + +template +class ValueOrPointeeLessTest : public ::testing::Test { +protected: + bool less(int lhs, int rhs) { + ValueOrPointeeLess comp; + return comp(create(lhs), create(rhs)); + } +}; +// set of template types to test for +typedef ::testing::Types TypeInstances; +TYPED_TEST_CASE(ValueOrPointeeLessTest, TypeInstances); +TYPED_TEST(ValueOrPointeeLessTest, less) { + EXPECT_TRUE(this->less(2, 3)); + EXPECT_FALSE(this->less(1, 1)); + EXPECT_FALSE(this->less(2, 1)); +} + +template +class OrderedTest : public ::testing::Test, public ordered { +protected: + void pushAndValidate(int cost, const std::vector& expected) { + this->insert(create(cost)); + + std::vector actual; + std::transform(this->c.begin(), this->c.end(), std::back_inserter(actual), + [](const T& item) -> int { return value(item); }); + + EXPECT_THAT(actual, ::testing::ElementsAreArray(expected)); + } + void validatePop() { + std::vector expected; + std::vector actual; + std::transform(this->c.begin(), this->c.end(), std::back_inserter(expected), + [](const T& item) -> int { return value(item); }); + + while(!this->empty()) + actual.push_back(value(this->pop())); + + EXPECT_THAT(actual, ::testing::ElementsAreArray(expected)); + EXPECT_TRUE(this->empty()); + } +}; + + +#define pushAndValidate(cost, ...) { \ + SCOPED_TRACE("pushAndValidate(" #cost ", " #__VA_ARGS__ ")"); \ + this->pushAndValidate(cost, __VA_ARGS__); \ +} +TYPED_TEST_CASE(OrderedTest, TypeInstances); +TYPED_TEST(OrderedTest, sorting) { + pushAndValidate(2, {2}); + pushAndValidate(1, {1,2}); + pushAndValidate(3, {1,2,3}); + this->validatePop(); + + pushAndValidate(1, {1}); + pushAndValidate(2, {1,2}); + pushAndValidate(3, {1,2,3}); + pushAndValidate(4, {1,2,3,4}); + pushAndValidate(5, {1,2,3,4,5}); + this->validatePop(); + + pushAndValidate(5, {5}); + pushAndValidate(4, {4,5}); + pushAndValidate(3, {3,4,5}); + pushAndValidate(1, {1,3,4,5}); + pushAndValidate(2, {1,2,3,4,5}); + this->validatePop(); +} template std::ostream& operator<<(std::ostream &os, const cost_ordered &queue) { diff --git a/visualization/motion_planning_tasks/test/CMakeLists.txt b/visualization/motion_planning_tasks/test/CMakeLists.txt index dbaa1fdd..b3a2cbdc 100644 --- a/visualization/motion_planning_tasks/test/CMakeLists.txt +++ b/visualization/motion_planning_tasks/test/CMakeLists.txt @@ -10,13 +10,11 @@ if (CATKIN_ENABLE_TESTING) target_link_libraries(${PROJECT_NAME}-test-merge-models motion_planning_tasks_utils gtest_main) + catkin_add_gtest(${PROJECT_NAME}-test-solution-models test_solution_models.cpp) + target_link_libraries(${PROJECT_NAME}-test-solution-models + motion_planning_tasks_rviz_plugin gtest_main) - if(NOT "${rviz_QT_VERSION}" VERSION_LESS "5") - # This tests uses Qt5's generic function slots - add_rostest_gtest(${PROJECT_NAME}-test-task_model test_task_model.launch test_task_model.cpp) - target_link_libraries(${PROJECT_NAME}-test-task_model - motion_planning_tasks_rviz_plugin - ${catkin_LIBRARIES} - gtest_main) - endif() + add_rostest_gtest(${PROJECT_NAME}-test-task_model test_task_model.launch test_task_model.cpp) + target_link_libraries(${PROJECT_NAME}-test-task_model + motion_planning_tasks_rviz_plugin ${catkin_LIBRARIES} gtest_main) endif() diff --git a/visualization/motion_planning_tasks/test/test_solution_models.cpp b/visualization/motion_planning_tasks/test/test_solution_models.cpp new file mode 100644 index 00000000..014c0840 --- /dev/null +++ b/visualization/motion_planning_tasks/test/test_solution_models.cpp @@ -0,0 +1,56 @@ +#include +#include +#include +#include + +using namespace moveit_rviz_plugin; + +class SolutionModelTest : public ::testing::Test { +protected: + template + inline std::vector reversed(const std::vector &in) { + return std::vector(in.rbegin(), in.rend()); + } + void validateSorting(QAbstractTableModel& model, int sort_column, Qt::SortOrder sort_order, + const std::vector& expected_id_order) { + SCOPED_TRACE(qPrintable(QString("sorting in %1 %2 order") + .arg(sort_order == Qt::AscendingOrder ? "ascending" : "descending") + .arg(sort_column == 0 ? "creation" : "cost"))); + model.sort(sort_column, sort_order); + std::vector actual_id_order(model.rowCount()); + for (int row = 0; row < model.rowCount(); ++row) + actual_id_order[row] = model.data(model.index(row, 0), Qt::UserRole).toInt(); + EXPECT_THAT(actual_id_order, ::testing::ElementsAreArray(expected_id_order)); + } + void processAndValidate(RemoteSolutionModel& model, + const std::vector& success_ids, + const std::vector& failure_ids) { + model.processSolutionIDs(success_ids, failure_ids, failure_ids.size()); + + std::vector cost_ordered_ids(success_ids.begin(), success_ids.end()); + std::vector sorted_failure_ids(failure_ids.begin(), failure_ids.end()); + std::sort(sorted_failure_ids.begin(), sorted_failure_ids.end()); + std::copy(sorted_failure_ids.begin(), sorted_failure_ids.end(), std::back_inserter(cost_ordered_ids)); + + std::vector creation_ordered_ids(cost_ordered_ids.size()); + for (size_t i=0; i < cost_ordered_ids.size(); ++i) + creation_ordered_ids[i] = i+1; + + validateSorting(model, 0, Qt::AscendingOrder, creation_ordered_ids); + validateSorting(model, 0, Qt::DescendingOrder, reversed(creation_ordered_ids)); + + validateSorting(model, 1, Qt::AscendingOrder, cost_ordered_ids); + validateSorting(model, 1, Qt::DescendingOrder, reversed(cost_ordered_ids)); + } +}; + +#define processAndValidate(...) { \ + SCOPED_TRACE("processSolutionIDs(" #__VA_ARGS__ ")"); \ + processAndValidate(model, __VA_ARGS__); \ +} + +TEST_F(SolutionModelTest, sorting) { + RemoteSolutionModel model; + processAndValidate({1,3}, {2}); + processAndValidate({4,1,6,3}, {5,2}); +}