Merge pull request #495 from ubi-agni/debug-#485

[WIP] Debug/Fix #485
This commit is contained in:
Michael Görner 2024-02-16 12:27:36 +01:00 committed by GitHub
commit 7638e5ff8b
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
13 changed files with 188 additions and 40 deletions

View File

@ -184,6 +184,7 @@ public:
void setName(const std::string& name);
uint32_t introspectionId() const;
Introspection* introspection() const;
/** set computation timeout (in seconds)
*

View File

@ -301,10 +301,20 @@ private:
};
PIMPL_FUNCTIONS(MonitoringGenerator)
// Print pending pairs of a ConnectingPrivate instance
class ConnectingPrivate;
struct PendingPairsPrinter
{
const ConnectingPrivate* const instance_;
PendingPairsPrinter(const ConnectingPrivate* c) : instance_(c) {}
};
std::ostream& operator<<(std::ostream& os, const PendingPairsPrinter& printer);
class ConnectingPrivate : public ComputeBasePrivate
{
friend class Connecting;
friend struct FallbacksPrivateConnect;
friend std::ostream& operator<<(std::ostream& os, const PendingPairsPrinter& printer);
public:
struct StatePair : std::pair<Interface::const_iterator, Interface::const_iterator>
@ -337,7 +347,7 @@ public:
template <Interface::Direction dir>
bool hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const;
std::ostream& printPendingPairs(std::ostream& os = std::cerr) const;
PendingPairsPrinter pendingPairsPrinter() const { return PendingPairsPrinter(this); }
private:
// Create a pair of Interface states for pending list, such that the order (start, end) is maintained
@ -352,5 +362,6 @@ private:
ordered<StatePair> pending;
};
PIMPL_FUNCTIONS(Connecting)
} // namespace task_constructor
} // namespace moveit

View File

@ -66,7 +66,7 @@ printChildrenInterfaces(const ContainerBasePrivate& container, bool success, con
if (success)
os << ++id << ' ';
if (const auto conn = dynamic_cast<const ConnectingPrivate*>(creator.pimpl()))
conn->printPendingPairs(os);
os << conn->pendingPairsPrinter();
os << std::endl;
for (const auto& child : container.children()) {
@ -487,7 +487,8 @@ struct SolutionCollector
solutions.emplace_back(std::make_pair(trace, prio));
} else {
for (SolutionBase* successor : next) {
assert(!successor->isFailure()); // We shouldn't have invalid solutions
if (successor->isFailure())
continue; // skip failures
trace.push_back(successor);
traverse(*successor, prio + InterfaceState::Priority(1, successor->cost()));
trace.pop_back();

View File

@ -55,6 +55,8 @@ bool isValidCharInName(char c); // unfortunately this is not declared in ros/na
} // namespace names
} // namespace ros
static const char* LOGGER = "introspection";
namespace moveit {
namespace task_constructor {
@ -206,6 +208,9 @@ uint32_t Introspection::stageId(const Stage* const s) const {
uint32_t Introspection::solutionId(const SolutionBase& s) {
auto result = impl->id_solution_bimap_.left.insert(std::make_pair(1 + impl->id_solution_bimap_.size(), &s));
if (result.second) // new entry
ROS_DEBUG_STREAM_NAMED(LOGGER, "new solution #" << result.first->first << " (" << s.creator()->name()
<< "): " << s.cost() << " " << s.comment());
return result.first->first;
}

View File

@ -381,6 +381,9 @@ uint32_t Stage::introspectionId() const {
throw std::runtime_error("Task is not initialized yet or Introspection was disabled.");
return const_cast<const moveit::task_constructor::Introspection*>(pimpl_->introspection_)->stageId(this);
}
Introspection* Stage::introspection() const {
return pimpl_->introspection_;
}
void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest) {
const PropertyMap& src = source.properties();
@ -791,20 +794,29 @@ void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags
assert(it->priority().enabled()); // new solutions are feasible, aren't they?
InterfacePtr other_interface = pullInterface<dir>();
bool have_enabled_opposites = false;
// other interface states to re-enable (post-poned because otherwise order in other_interface changes during loop)
std::vector<Interface::iterator> oit_to_enable;
for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) {
if (!static_cast<Connecting*>(me_)->compatible(*it, *oit))
continue;
// re-enable the opposing state oit (and its associated solution branch) if its status is ARMED
// https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202
if (oit->priority().status() == InterfaceState::Status::ARMED)
parent_pimpl->setStatus<opposite<dir>()>(me(), &*it, &*oit, InterfaceState::Status::ENABLED);
if (oit->priority().status() == InterfaceState::Status::ARMED) {
oit_to_enable.push_back(oit);
have_enabled_opposites = true;
}
if (oit->priority().enabled())
have_enabled_opposites = true;
// Remember all pending states, regardless of their status!
pending.insert(make_pair<dir>(it, oit));
}
// actually re-enable other interface states, which were scheduled for re-enabling above
for (Interface::iterator oit : oit_to_enable)
parent_pimpl->setStatus<opposite<dir>()>(me(), &*it, &*oit, InterfaceState::Status::ENABLED);
if (!have_enabled_opposites) // prune new state and associated branch if necessary
// pass creator=nullptr to skip hasPendingOpposites() check as we did this here already
parent_pimpl->setStatus<dir>(nullptr, nullptr, &*it, InterfaceState::Status::ARMED);
@ -823,7 +835,7 @@ void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags
os << d << " " << this->pullInterface(d) << ": " << *this->pullInterface(d) << std::endl;
}
os << std::setw(15) << " ";
printPendingPairs(os) << std::endl;
os << pendingPairsPrinter() << std::endl;
#endif
}
@ -853,6 +865,7 @@ template bool ConnectingPrivate::hasPendingOpposites<Interface::BACKWARD>(const
const InterfaceState* start) const;
bool ConnectingPrivate::canCompute() const {
// ROS_DEBUG_STREAM("canCompute " << name() << ": " << pendingPairsPrinter());
// Do we still have feasible pending state pairs?
return !pending.empty() && pending.front().first->priority().enabled() &&
pending.front().second->priority().enabled();
@ -866,15 +879,16 @@ void ConnectingPrivate::compute() {
static_cast<Connecting*>(me_)->compute(from, to);
}
std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const {
std::ostream& operator<<(std::ostream& os, const PendingPairsPrinter& p) {
const auto* impl = p.instance_;
const char* reset = InterfaceState::colorForStatus(3);
for (const auto& candidate : pending) {
size_t first = getIndex(*starts(), candidate.first);
size_t second = getIndex(*ends(), candidate.second);
for (const auto& candidate : impl->pending) {
size_t first = getIndex(*impl->starts(), candidate.first);
size_t second = getIndex(*impl->ends(), candidate.second);
os << InterfaceState::colorForStatus(candidate.first->priority().status()) << first << reset << ":"
<< InterfaceState::colorForStatus(candidate.second->priority().status()) << second << reset << " ";
}
if (pending.empty())
if (impl->pending.empty())
os << "---";
return os;
}

View File

@ -34,9 +34,10 @@ if (CATKIN_ENABLE_TESTING)
mtc_add_test("gmock" ${ARGN})
endmacro()
mtc_add_gmock(test_mockups.cpp)
mtc_add_gtest(test_stage.cpp)
mtc_add_gtest(test_container.cpp)
mtc_add_gtest(test_serial.cpp)
mtc_add_gmock(test_serial.cpp)
mtc_add_gtest(test_pruning.cpp)
mtc_add_gtest(test_properties.cpp)
mtc_add_gtest(test_cost_terms.cpp)

View File

@ -46,6 +46,19 @@ double PredefinedCosts::cost() const {
return c;
}
void DelayingWrapper::compute() {
if (!delay_.empty()) { // if empty, we don't delay
if (delay_.front() == 0)
delay_.pop_front(); // we can compute() now
else {
--delay_.front(); // continue delaying
return;
}
}
// forward to child, eventually generating multiple solutions at once
WrapperBase::compute();
}
GeneratorMockup::GeneratorMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
: Generator{ "GEN" + std::to_string(++id_) }
, costs_{ std::move(costs) }
@ -86,6 +99,7 @@ void ConnectMockup::compute(const InterfaceState& from, const InterfaceState& to
auto solution{ std::make_shared<SubTrajectory>() };
solution->setCost(costs_.cost());
solution->setComment(std::to_string(from.priority().cost()) + " -> " + std::to_string(to.priority().cost()));
connect(from, to, solution);
}

View File

@ -34,6 +34,18 @@ struct PredefinedCosts : CostTerm
constexpr double INF{ std::numeric_limits<double>::infinity() };
/* wrapper stage to delay solutions by a given number of steps */
struct DelayingWrapper : public WrapperBase
{
std::list<unsigned int> delay_;
/* delay list specifies the number of steps each received solution should be delayed */
DelayingWrapper(std::list<unsigned int> delay, Stage::pointer&& child)
: WrapperBase("delayer", std::move(child)), delay_{ std::move(delay) } {}
void compute() override;
void onNewSolution(const SolutionBase& s) override { liftSolution(s); }
};
struct GeneratorMockup : public Generator
{
planning_scene::PlanningScenePtr ps_;
@ -47,8 +59,8 @@ struct GeneratorMockup : public Generator
// default to one solution to avoid infinity loops
GeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ std::list<double>{ 0.0 }, true },
std::size_t solutions_per_compute = 1);
GeneratorMockup(std::initializer_list<double> costs)
: GeneratorMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
GeneratorMockup(std::initializer_list<double> costs, std::size_t solutions_per_compute = 1)
: GeneratorMockup{ PredefinedCosts{ std::list<double>{ costs }, true }, solutions_per_compute } {}
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override;

View File

@ -110,7 +110,7 @@ protected:
Container container;
InterfacePtr dummy;
InitTest() : ::testing::Test{}, robot_model{ getModel() }, dummy{ new Interface } {}
InitTest() : ::testing::Test{}, robot_model{ getModel() }, dummy{ std::make_shared<Interface>() } {}
void pushInterface(bool start = true, bool end = true) {
// pretend, that the container is connected

View File

@ -0,0 +1,76 @@
#include "stage_mockups.h"
#include <moveit/task_constructor/container_p.h>
using namespace moveit::task_constructor;
struct TestGeneratorMockup : public GeneratorMockup
{
InterfacePtr next_starts_;
InterfacePtr prev_ends_;
using GeneratorMockup::GeneratorMockup;
using GeneratorMockup::init;
void init() {
next_starts_ = std::make_shared<Interface>();
prev_ends_ = std::make_shared<Interface>();
GeneratorMockup::reset();
GeneratorMockup::init(getModel());
GeneratorPrivate* impl = pimpl();
impl->setNextStarts(next_starts_);
impl->setPrevEnds(prev_ends_);
}
};
TEST(GeneratorMockup, compute) {
TestGeneratorMockup gen({ 1.0, 2.0, 3.0 });
gen.init();
for (int i = 0; i < 3; ++i) {
ASSERT_TRUE(gen.canCompute());
gen.compute();
}
EXPECT_FALSE(gen.canCompute());
EXPECT_COSTS(gen.solutions(), ::testing::ElementsAre(1, 2, 3));
}
#define COMPUTE_EXPECT_COSTS(stage, ...) \
{ \
EXPECT_TRUE(stage.canCompute()); \
stage.compute(); \
EXPECT_COSTS(stage.solutions(), ::testing::ElementsAre(__VA_ARGS__)); \
constexpr auto num_elements = std::initializer_list<double>{ __VA_ARGS__ }.size(); \
EXPECT_EQ(runs, num_elements); \
}
TEST(GeneratorMockup, delayed) {
auto gen = std::make_unique<TestGeneratorMockup>(PredefinedCosts({ 1.0, 2.0, 3.0 }));
gen->init();
auto& runs = gen->runs_;
DelayingWrapper w({ 1, 0, 2 }, std::move(gen));
auto prev_ends = std::make_shared<Interface>();
auto next_starts = std::make_shared<Interface>();
WrapperBasePrivate* impl = w.pimpl();
impl->setPrevEnds(prev_ends);
impl->setNextStarts(next_starts);
// 1st compute() is delayed by 1
COMPUTE_EXPECT_COSTS(w);
COMPUTE_EXPECT_COSTS(w, 1);
// 2nd compute() is not delayed
COMPUTE_EXPECT_COSTS(w, 1, 2);
// 1st compute() is delayed by 2
COMPUTE_EXPECT_COSTS(w, 1, 2);
COMPUTE_EXPECT_COSTS(w, 1, 2);
COMPUTE_EXPECT_COSTS(w, 1, 2, 3);
EXPECT_FALSE(w.canCompute());
}
#undef COMPUTE_EXPECT_COSTS

View File

@ -6,8 +6,6 @@
#include <list>
#include <memory>
#include <gtest/gtest.h>
#ifndef TYPED_TEST_SUITE
#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES)
#endif
@ -100,14 +98,7 @@ TEST_F(Pruning, ConnectConnectForward) {
add(t, new GeneratorMockup({ 1, 2, 3 }));
t.plan();
ASSERT_EQ(t.solutions().size(), 3u * 2u);
std::vector<double> expected_costs = { 11, 12, 13, 21, 22, 23 };
auto expected_cost = expected_costs.begin();
for (const auto& s : t.solutions()) {
EXPECT_EQ(s->cost(), *expected_cost);
++expected_cost;
}
EXPECT_COSTS(t.solutions(), ::testing::ElementsAre(11, 12, 13, 21, 22, 23));
EXPECT_EQ(c1->runs_, 3u);
EXPECT_EQ(c2->runs_, 6u); // expect 6 instead of 9 calls
}
@ -123,13 +114,7 @@ TEST_F(Pruning, ConnectConnectBackward) {
t.plan();
ASSERT_EQ(t.solutions().size(), 3u * 2u);
std::vector<double> expected_costs = { 11, 12, 13, 21, 22, 23 };
auto expected_cost = expected_costs.begin();
for (const auto& s : t.solutions()) {
EXPECT_EQ(s->cost(), *expected_cost);
++expected_cost;
}
EXPECT_COSTS(t.solutions(), ::testing::ElementsAre(11, 12, 13, 21, 22, 23));
EXPECT_EQ(c1->runs_, 6u); // expect 6 instead of 9 calls
EXPECT_EQ(c2->runs_, 3u);
}
@ -207,3 +192,18 @@ TEST_F(Pruning, TwoConnects) {
EXPECT_FALSE(t.plan());
}
TEST_F(Pruning, BackPropagateFailure) {
add(t, new GeneratorMockup({ 1.0 }));
auto con1 = add(t, new ConnectMockup());
add(t, new GeneratorMockup({ 10.0, 20.0 }, 2)); // create all solutions on first run
auto con2 = add(t, new ConnectMockup());
add(t, new GeneratorMockup({ 100.0, 200.0 }, 2)); // create all solutions on first run
// delay failure (INF) until CON2 has found first solution
add(t, new DelayingWrapper({ 1 }, std::make_unique<ForwardMockup>(PredefinedCosts({ INF, 2000 }))));
EXPECT_TRUE(t.plan());
EXPECT_COSTS(t.solutions(), ::testing::ElementsAre(2211, 2221));
EXPECT_EQ(con1->runs_, 2u);
EXPECT_EQ(con2->runs_, 3u); // 100 - 20 is pruned
}

View File

@ -9,7 +9,6 @@
#include "models.h"
#include <list>
#include <memory>
#include <gtest/gtest.h>
using namespace moveit::task_constructor;
using namespace planning_scene;
@ -56,13 +55,7 @@ TEST_F(ConnectConnect, SuccSucc) {
add(t, new GeneratorMockup({ 0.0 }));
EXPECT_TRUE(t.plan());
ASSERT_EQ(t.solutions().size(), 3u * 2u);
std::vector<double> expected_costs = { 11, 12, 13, 21, 22, 23 };
auto expected_cost = expected_costs.begin();
for (const auto& s : t.solutions()) {
EXPECT_EQ(s->cost(), *expected_cost);
++expected_cost;
}
EXPECT_COSTS(t.solutions(), ::testing::ElementsAre(11, 12, 13, 21, 22, 23));
}
// https://github.com/ros-planning/moveit_task_constructor/issues/218
@ -76,3 +69,17 @@ TEST_F(ConnectConnect, FailSucc) {
EXPECT_FALSE(t.plan());
}
// https://github.com/ros-planning/moveit_task_constructor/issues/485#issuecomment-1760606116
TEST_F(ConnectConnect, UniqueEnumeration) {
add(t, new GeneratorMockup({ 1.0, 2.0, 3.0 }));
auto con1 = add(t, new ConnectMockup());
add(t, new GeneratorMockup({ 10.0, 20.0 }));
auto con2 = add(t, new ConnectMockup({ INF, 0.0, 0.0, 0.0 }));
add(t, new GeneratorMockup({ 100.0, 200.0 }));
EXPECT_TRUE(t.plan());
EXPECT_COSTS(t.solutions(), ::testing::ElementsAre(121, 122, 123, 211, 212, 213, 221, 222, 223));
EXPECT_EQ(con1->runs_, 3u * 2u);
EXPECT_EQ(con2->runs_, 2u * 2u);
}

View File

@ -501,7 +501,13 @@ QVariant RemoteSolutionModel::data(const QModelIndex& index, int role) const {
return item.id;
case Qt::ToolTipRole:
#if 0 // show internal solution id in first column
if (index.column() == 0)
return item.id;
#endif
// usually just show the comment
return item.comment;
break;
case Qt::DisplayRole:
switch (index.column()) {