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"; - } }