From 45e6cdb47db40025dc9de3ff887f6079289e921c Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 14 Sep 2020 19:24:01 +0200 Subject: [PATCH] Revert "add CostAggregator for SerialContainer" This reverts commit dc7ce9bdfac97eb468f5a850adcb27cc118b5fd7. It turns out multiple places in SerialContainer's cost inference expect 0.0 as the neutral element (which is why std::min and multiply did not work). While these additional issues can be fixed, it would make the interface much less elegant. We should consider adding it back if an actual use-case is there to discuss. --- .../moveit/task_constructor/container.h | 6 --- .../moveit/task_constructor/container_p.h | 2 - .../include/moveit/task_constructor/storage.h | 10 ++--- core/src/container.cpp | 22 +++------ core/src/storage.cpp | 12 +---- core/test/test_cost_terms.cpp | 45 ------------------- 6 files changed, 10 insertions(+), 87 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 8ca032e7..27b2fde0 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -94,12 +94,6 @@ public: bool canCompute() const override; void compute() override; - /** Specify how to aggregate costs of childrens' solutions - * - * By default costs are added up - */ - void setCostAggregator(const SolutionSequence::CostAggregator& agg); - protected: /// called by a (direct) child when a new solution becomes available void onNewSolution(const SolutionBase& s) override; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index cad54fcd..35a903ad 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -184,8 +184,6 @@ protected: // validate that child's interface matches mine (considering start or end only as determined by mask) template void validateInterface(const StagePrivate& child, InterfaceFlags required) const; - - SolutionSequence::CostAggregator aggregator_; }; PIMPL_FUNCTIONS(SerialContainer) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 52138ae2..5bf81a1f 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -318,18 +318,15 @@ class SolutionSequence : public SolutionBase public: using container_type = std::vector; - explicit SolutionSequence(); - - SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr); + explicit SolutionSequence() : SolutionBase() {} + SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr) + : SolutionBase(creator, cost), subsolutions_(std::move(subsolutions)) {} void push_back(const SolutionBase& solution); /// append all subsolutions to solution void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; - using CostAggregator = std::function; - void setCostAggregator(const CostAggregator&); - /// aggregate costs along the sequence double computeCost(const CostTerm& cost, std::string& comment) const override; @@ -339,7 +336,6 @@ public: inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); } private: - CostAggregator aggregator_; /// series of sub solutions container_type subsolutions_; }; diff --git a/core/src/container.cpp b/core/src/container.cpp index 1c1b0993..18892591 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -360,9 +360,8 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { solution.reserve(children.size()); for (auto& in : incoming.solutions) { for (auto& out : outgoing.solutions) { - InterfaceState::Priority prio( - static_cast(in.first.size() + 1 + out.first.size()), - pimpl()->aggregator_(pimpl()->aggregator_(in.second, current.cost()), out.second)); + InterfaceState::Priority prio(static_cast(in.first.size() + 1 + out.first.size()), + in.second + current.cost() + out.second); // found a complete solution path connecting start to end? if (prio.depth() == children.size()) { if (std::isinf(prio.cost())) @@ -375,9 +374,7 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { // insert outgoing solutions in normal order solution.insert(solution.end(), out.first.begin(), out.first.end()); // store solution in sorted list - auto sequence{ std::make_shared(std::move(solution), prio.cost(), this) }; - sequence->setCostAggregator(pimpl()->aggregator_); - sorted.insert(std::move(sequence)); + sorted.insert(std::make_shared(std::move(solution), prio.cost(), this)); } else if (prio.depth() > 1) { // update state priorities along the whole partial solution path updateStateCosts(in.first, prio); @@ -396,7 +393,7 @@ SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(i SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {} SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name) - : ContainerBasePrivate(me, name), aggregator_(std::plus{}) {} + : ContainerBasePrivate(me, name) {} void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) { InterfaceFlags flags1 = stage1.requiredInterface(); @@ -521,10 +518,6 @@ void SerialContainerPrivate::validateConnectivity() const { } } -void SerialContainer::setCostAggregator(const SolutionSequence::CostAggregator& agg) { - pimpl()->aggregator_ = agg; -} - bool SerialContainer::canCompute() const { for (const auto& stage : pimpl()->children()) { if (stage->pimpl()->canCompute()) @@ -556,14 +549,11 @@ void SerialContainer::traverse(const SolutionBase& start, const SolutionProcesso else for (SolutionBase* successor : solutions) { trace.push_back(successor); - - double previous_trace_cost { trace_cost }; - - trace_cost = pimpl()->aggregator_(trace_cost, successor->cost()); + trace_cost += successor->cost(); traverse(*successor, cb, trace, trace_cost); - trace_cost = previous_trace_cost; + trace_cost -= successor->cost(); trace.pop_back(); } } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 1133257e..ff096a12 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -182,19 +182,10 @@ double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const return f(*this, comment); } -SolutionSequence::SolutionSequence() : SolutionBase(), aggregator_(std::plus{}) {} - -SolutionSequence::SolutionSequence(container_type&& subsolutions, double cost, Stage* creator) - : SolutionBase(creator, cost), aggregator_(std::plus{}), subsolutions_(std::move(subsolutions)) {} - void SolutionSequence::push_back(const SolutionBase& solution) { subsolutions_.push_back(&solution); } -void SolutionSequence::setCostAggregator(const CostAggregator& agg) { - aggregator_ = agg; -} - void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { moveit_task_constructor_msgs::SubSolution sub_msg; SolutionBase::fillInfo(sub_msg.info, introspection); @@ -234,8 +225,7 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co double cost{ 0.0 }; std::string subcomment; for (auto& solution : subsolutions_) { - auto subcost{ solution->computeCost(f, subcomment) }; - cost = aggregator_(cost, subcost); + cost += solution->computeCost(f, subcomment); if (!subcomment.empty()) { if (!comment.empty()) comment.append(", "); diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index c819c343..7809a81b 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -11,8 +11,6 @@ #include -#include - using namespace moveit::task_constructor; using namespace planning_scene; @@ -351,47 +349,4 @@ TEST(CostTerm, CompositeSolutionsContainerCost) { << "container cost term overwrites stage costs"; EXPECT_EQ(s1_ptr->solutions().front()->cost(), STAGE_COST) << "child cost is not affected"; } - -TEST(CostTerm, CompositeSolutionsContainerAggregator) { - ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); - Standalone container{ getModel() }; - - cost::Constant constant{ 17.0 }; - - { - auto s1{ std::make_unique() }; - auto s2{ std::make_unique() }; - auto s3{ std::make_unique() }; - - container.computeWithStageCost({ std::move(s1), std::move(s2), std::move(s3) }, constant); - EXPECT_EQ(container.solutions().front()->cost(), 3 * constant.cost) << "default cost aggregates through stages"; - } - - { - container.setCostAggregator([](double a, double b) { return std::max(a, b); }); - container.computeWithContainerCost( - { - std::make_unique(), - std::make_unique(), - std::make_unique(), - }, - constant); - - EXPECT_EQ(container.solutions().front()->cost(), constant.cost) - << "custom aggregator works with explicit cost term in container"; - } - - { - container.setCostAggregator([](auto a, auto b) { return std::max(a, b); }); - container.computeWithStageCost( - { - std::make_unique(), - std::make_unique(), - std::make_unique(), - }, - constant); - - EXPECT_EQ(container.solutions().front()->cost(), constant.cost) - << "custom aggregator works without container cost term"; - } }