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.
This commit is contained in:
v4hn 2020-09-14 19:24:01 +02:00
parent cce7380f9d
commit 45e6cdb47d
6 changed files with 10 additions and 87 deletions

View File

@ -94,12 +94,6 @@ public:
bool canCompute() const override; bool canCompute() const override;
void compute() 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: protected:
/// called by a (direct) child when a new solution becomes available /// called by a (direct) child when a new solution becomes available
void onNewSolution(const SolutionBase& s) override; void onNewSolution(const SolutionBase& s) override;

View File

@ -184,8 +184,6 @@ protected:
// validate that child's interface matches mine (considering start or end only as determined by mask) // validate that child's interface matches mine (considering start or end only as determined by mask)
template <unsigned int mask> template <unsigned int mask>
void validateInterface(const StagePrivate& child, InterfaceFlags required) const; void validateInterface(const StagePrivate& child, InterfaceFlags required) const;
SolutionSequence::CostAggregator aggregator_;
}; };
PIMPL_FUNCTIONS(SerialContainer) PIMPL_FUNCTIONS(SerialContainer)

View File

@ -318,18 +318,15 @@ class SolutionSequence : public SolutionBase
public: public:
using container_type = std::vector<const SolutionBase*>; using container_type = std::vector<const SolutionBase*>;
explicit SolutionSequence(); explicit SolutionSequence() : SolutionBase() {}
SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr)
SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr); : SolutionBase(creator, cost), subsolutions_(std::move(subsolutions)) {}
void push_back(const SolutionBase& solution); void push_back(const SolutionBase& solution);
/// append all subsolutions to solution /// append all subsolutions to solution
void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override;
using CostAggregator = std::function<double(double, double)>;
void setCostAggregator(const CostAggregator&);
/// aggregate costs along the sequence /// aggregate costs along the sequence
double computeCost(const CostTerm& cost, std::string& comment) const override; double computeCost(const CostTerm& cost, std::string& comment) const override;
@ -339,7 +336,6 @@ public:
inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); } inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); }
private: private:
CostAggregator aggregator_;
/// series of sub solutions /// series of sub solutions
container_type subsolutions_; container_type subsolutions_;
}; };

View File

@ -360,9 +360,8 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
solution.reserve(children.size()); solution.reserve(children.size());
for (auto& in : incoming.solutions) { for (auto& in : incoming.solutions) {
for (auto& out : outgoing.solutions) { for (auto& out : outgoing.solutions) {
InterfaceState::Priority prio( InterfaceState::Priority prio(static_cast<unsigned int>(in.first.size() + 1 + out.first.size()),
static_cast<unsigned int>(in.first.size() + 1 + out.first.size()), in.second + current.cost() + out.second);
pimpl()->aggregator_(pimpl()->aggregator_(in.second, current.cost()), out.second));
// found a complete solution path connecting start to end? // found a complete solution path connecting start to end?
if (prio.depth() == children.size()) { if (prio.depth() == children.size()) {
if (std::isinf(prio.cost())) if (std::isinf(prio.cost()))
@ -375,9 +374,7 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
// insert outgoing solutions in normal order // insert outgoing solutions in normal order
solution.insert(solution.end(), out.first.begin(), out.first.end()); solution.insert(solution.end(), out.first.begin(), out.first.end());
// store solution in sorted list // store solution in sorted list
auto sequence{ std::make_shared<SolutionSequence>(std::move(solution), prio.cost(), this) }; sorted.insert(std::make_shared<SolutionSequence>(std::move(solution), prio.cost(), this));
sequence->setCostAggregator(pimpl()->aggregator_);
sorted.insert(std::move(sequence));
} else if (prio.depth() > 1) { } else if (prio.depth() > 1) {
// update state priorities along the whole partial solution path // update state priorities along the whole partial solution path
updateStateCosts(in.first, prio); 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)) {} SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {}
SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name) SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name)
: ContainerBasePrivate(me, name), aggregator_(std::plus<double>{}) {} : ContainerBasePrivate(me, name) {}
void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) { void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) {
InterfaceFlags flags1 = stage1.requiredInterface(); 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 { bool SerialContainer::canCompute() const {
for (const auto& stage : pimpl()->children()) { for (const auto& stage : pimpl()->children()) {
if (stage->pimpl()->canCompute()) if (stage->pimpl()->canCompute())
@ -556,14 +549,11 @@ void SerialContainer::traverse(const SolutionBase& start, const SolutionProcesso
else else
for (SolutionBase* successor : solutions) { for (SolutionBase* successor : solutions) {
trace.push_back(successor); trace.push_back(successor);
trace_cost += successor->cost();
double previous_trace_cost { trace_cost };
trace_cost = pimpl()->aggregator_(trace_cost, successor->cost());
traverse<dir>(*successor, cb, trace, trace_cost); traverse<dir>(*successor, cb, trace, trace_cost);
trace_cost = previous_trace_cost; trace_cost -= successor->cost();
trace.pop_back(); trace.pop_back();
} }
} }

View File

@ -182,19 +182,10 @@ double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const
return f(*this, comment); return f(*this, comment);
} }
SolutionSequence::SolutionSequence() : SolutionBase(), aggregator_(std::plus<double>{}) {}
SolutionSequence::SolutionSequence(container_type&& subsolutions, double cost, Stage* creator)
: SolutionBase(creator, cost), aggregator_(std::plus<double>{}), subsolutions_(std::move(subsolutions)) {}
void SolutionSequence::push_back(const SolutionBase& solution) { void SolutionSequence::push_back(const SolutionBase& solution) {
subsolutions_.push_back(&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 { void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const {
moveit_task_constructor_msgs::SubSolution sub_msg; moveit_task_constructor_msgs::SubSolution sub_msg;
SolutionBase::fillInfo(sub_msg.info, introspection); SolutionBase::fillInfo(sub_msg.info, introspection);
@ -234,8 +225,7 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co
double cost{ 0.0 }; double cost{ 0.0 };
std::string subcomment; std::string subcomment;
for (auto& solution : subsolutions_) { for (auto& solution : subsolutions_) {
auto subcost{ solution->computeCost(f, subcomment) }; cost += solution->computeCost(f, subcomment);
cost = aggregator_(cost, subcost);
if (!subcomment.empty()) { if (!subcomment.empty()) {
if (!comment.empty()) if (!comment.empty())
comment.append(", "); comment.append(", ");

View File

@ -11,8 +11,6 @@
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <algorithm>
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
using namespace planning_scene; using namespace planning_scene;
@ -351,47 +349,4 @@ TEST(CostTerm, CompositeSolutionsContainerCost) {
<< "container cost term overwrites stage costs"; << "container cost term overwrites stage costs";
EXPECT_EQ(s1_ptr->solutions().front()->cost(), STAGE_COST) << "child cost is not affected"; 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<SerialContainer> container{ getModel() };
cost::Constant constant{ 17.0 };
{
auto s1{ std::make_unique<ForwardMockup>() };
auto s2{ std::make_unique<ForwardMockup>() };
auto s3{ std::make_unique<ForwardMockup>() };
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<ForwardMockup>(),
std::make_unique<ForwardMockup>(),
std::make_unique<ForwardMockup>(),
},
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<ForwardMockup>(),
std::make_unique<ForwardMockup>(),
std::make_unique<ForwardMockup>(),
},
constant);
EXPECT_EQ(container.solutions().front()->cost(), constant.cost)
<< "custom aggregator works without container cost term";
}
} }