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

View File

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

View File

@ -318,18 +318,15 @@ class SolutionSequence : public SolutionBase
public:
using container_type = std::vector<const SolutionBase*>;
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<double(double, double)>;
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_;
};

View File

@ -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<unsigned int>(in.first.size() + 1 + out.first.size()),
pimpl()->aggregator_(pimpl()->aggregator_(in.second, current.cost()), out.second));
InterfaceState::Priority prio(static_cast<unsigned int>(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<SolutionSequence>(std::move(solution), prio.cost(), this) };
sequence->setCostAggregator(pimpl()->aggregator_);
sorted.insert(std::move(sequence));
sorted.insert(std::make_shared<SolutionSequence>(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<double>{}) {}
: 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<dir>(*successor, cb, trace, trace_cost);
trace_cost = previous_trace_cost;
trace_cost -= successor->cost();
trace.pop_back();
}
}

View File

@ -182,19 +182,10 @@ double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const
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) {
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(", ");

View File

@ -11,8 +11,6 @@
#include <gtest/gtest.h>
#include <algorithm>
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<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";
}
}