From 0fd989cb27ee274299e7b461439c8686a9e32f47 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 12 Oct 2023 17:03:06 +0200 Subject: [PATCH 1/8] Simplify tests --- core/test/CMakeLists.txt | 2 +- core/test/test_pruning.cpp | 19 ++----------------- core/test/test_serial.cpp | 9 +-------- 3 files changed, 4 insertions(+), 26 deletions(-) diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 3918b584..9125df02 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -36,7 +36,7 @@ if (CATKIN_ENABLE_TESTING) 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) diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index b26ac527..9c15d1a3 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -6,8 +6,6 @@ #include #include -#include - #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 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 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); } diff --git a/core/test/test_serial.cpp b/core/test/test_serial.cpp index c8d333fb..764b7998 100644 --- a/core/test/test_serial.cpp +++ b/core/test/test_serial.cpp @@ -9,7 +9,6 @@ #include "models.h" #include #include -#include 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 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 From 605a1e54194231872b6d023cc44ea85778605d4a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 19 Oct 2023 19:08:57 +0200 Subject: [PATCH 2/8] DelayingWrapper stage to delay solution shipping in unit tests --- core/test/CMakeLists.txt | 1 + core/test/stage_mockups.cpp | 13 ++++++ core/test/stage_mockups.h | 16 +++++++- core/test/test_container.cpp | 2 +- core/test/test_mockups.cpp | 76 ++++++++++++++++++++++++++++++++++++ 5 files changed, 105 insertions(+), 3 deletions(-) create mode 100644 core/test/test_mockups.cpp diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 9125df02..b9709651 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -34,6 +34,7 @@ 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_gmock(test_serial.cpp) diff --git a/core/test/stage_mockups.cpp b/core/test/stage_mockups.cpp index 463ca360..b084faf3 100644 --- a/core/test/stage_mockups.cpp +++ b/core/test/stage_mockups.cpp @@ -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) } diff --git a/core/test/stage_mockups.h b/core/test/stage_mockups.h index f7bec9a0..fc75c633 100644 --- a/core/test/stage_mockups.h +++ b/core/test/stage_mockups.h @@ -34,6 +34,18 @@ struct PredefinedCosts : CostTerm constexpr double INF{ std::numeric_limits::infinity() }; +/* wrapper stage to delay solutions by a given number of steps */ +struct DelayingWrapper : public WrapperBase +{ + std::list delay_; + /* delay list specifies the number of steps each received solution should be delayed */ + DelayingWrapper(std::list 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{ 0.0 }, true }, std::size_t solutions_per_compute = 1); - GeneratorMockup(std::initializer_list costs) - : GeneratorMockup{ PredefinedCosts{ std::list{ costs }, true } } {} + GeneratorMockup(std::initializer_list costs, std::size_t solutions_per_compute = 1) + : GeneratorMockup{ PredefinedCosts{ std::list{ costs }, true }, solutions_per_compute } {} void init(const moveit::core::RobotModelConstPtr& robot_model) override; bool canCompute() const override; diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 1a7a9de4..876a1e6d 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -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() } {} void pushInterface(bool start = true, bool end = true) { // pretend, that the container is connected diff --git a/core/test/test_mockups.cpp b/core/test/test_mockups.cpp new file mode 100644 index 00000000..1eae5a85 --- /dev/null +++ b/core/test/test_mockups.cpp @@ -0,0 +1,76 @@ +#include "stage_mockups.h" +#include + +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(); + prev_ends_ = std::make_shared(); + + 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{ __VA_ARGS__ }.size(); \ + EXPECT_EQ(runs, num_elements); \ + } + +TEST(GeneratorMockup, delayed) { + auto gen = std::make_unique(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(); + auto next_starts = std::make_shared(); + + 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 From 08b102d5b07e31ea1cd940a652833cdaf285e61a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 20 Oct 2023 06:27:23 +0200 Subject: [PATCH 3/8] Unit tests for #485 --- core/test/test_pruning.cpp | 15 +++++++++++++++ core/test/test_serial.cpp | 16 ++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index 9c15d1a3..60e5d54a 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -192,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(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 +} diff --git a/core/test/test_serial.cpp b/core/test/test_serial.cpp index 764b7998..406b6663 100644 --- a/core/test/test_serial.cpp +++ b/core/test/test_serial.cpp @@ -69,3 +69,19 @@ TEST_F(ConnectConnect, FailSucc) { EXPECT_FALSE(t.plan()); } + +// https://github.com/ros-planning/moveit_task_constructor/issues/485#issuecomment-1760606116 +// TODO: Solutions are enumerated multiple times +// TODO: invalid solutions leak into enumeration +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); +} From 7b965863e30f2867b09e0bbddd9ddd50056d232c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 13 Oct 2023 01:30:07 +0200 Subject: [PATCH 4/8] Add more debugging output - Add Stage::introspection() accessor - Introspection: debug-log solution registration - RemoteSolutionModel: show internal solution id as tooltip in 1st column --- core/include/moveit/task_constructor/stage.h | 1 + core/src/introspection.cpp | 5 +++++ core/src/stage.cpp | 3 +++ core/test/stage_mockups.cpp | 1 + .../motion_planning_tasks/src/remote_task_model.cpp | 10 +++++++++- 5 files changed, 19 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 4d1a6706..e7f74f31 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -184,6 +184,7 @@ public: void setName(const std::string& name); uint32_t introspectionId() const; + Introspection* introspection() const; /** set computation timeout (in seconds) * diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 9820efed..d46542e1 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -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; } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 3298b3ad..2f7fcb1c 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -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(pimpl_->introspection_)->stageId(this); } +Introspection* Stage::introspection() const { + return pimpl_->introspection_; +} void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest) { const PropertyMap& src = source.properties(); diff --git a/core/test/stage_mockups.cpp b/core/test/stage_mockups.cpp index b084faf3..9397be56 100644 --- a/core/test/stage_mockups.cpp +++ b/core/test/stage_mockups.cpp @@ -99,6 +99,7 @@ void ConnectMockup::compute(const InterfaceState& from, const InterfaceState& to auto solution{ std::make_shared() }; solution->setCost(costs_.cost()); + solution->setComment(std::to_string(from.priority().cost()) + " -> " + std::to_string(to.priority().cost())); connect(from, to, solution); } diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index d34cb40b..8ab98047 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -501,7 +501,15 @@ QVariant RemoteSolutionModel::data(const QModelIndex& index, int role) const { return item.id; case Qt::ToolTipRole: - return item.comment; + switch (index.column()) { +#if 1 // show internal solution id in first column return item + case 0: + return item.id; + default: + return item.comment; +#endif + } + break; case Qt::DisplayRole: switch (index.column()) { From 13264ba21d8ea065b462699ee65e7f1355cc68c9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 20 Oct 2023 06:55:38 +0200 Subject: [PATCH 5/8] Fix leaking of failures into enumerated solutions --- core/src/container.cpp | 3 ++- core/test/test_serial.cpp | 1 - 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 10d28105..4203f3e2 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -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(); diff --git a/core/test/test_serial.cpp b/core/test/test_serial.cpp index 406b6663..9d81cf3c 100644 --- a/core/test/test_serial.cpp +++ b/core/test/test_serial.cpp @@ -72,7 +72,6 @@ TEST_F(ConnectConnect, FailSucc) { // https://github.com/ros-planning/moveit_task_constructor/issues/485#issuecomment-1760606116 // TODO: Solutions are enumerated multiple times -// TODO: invalid solutions leak into enumeration TEST_F(ConnectConnect, UniqueEnumeration) { add(t, new GeneratorMockup({ 1.0, 2.0, 3.0 })); auto con1 = add(t, new ConnectMockup()); From bf2714b42d7f2654b250c1c52dc2b4da73af0dca Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 20 Oct 2023 08:56:29 +0200 Subject: [PATCH 6/8] printPendingPairs(os) -> os< @@ -337,7 +347,7 @@ public: template 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 pending; }; PIMPL_FUNCTIONS(Connecting) + } // namespace task_constructor } // namespace moveit diff --git a/core/src/container.cpp b/core/src/container.cpp index 4203f3e2..0fe38c79 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -66,7 +66,7 @@ printChildrenInterfaces(const ContainerBasePrivate& container, bool success, con if (success) os << ++id << ' '; if (const auto conn = dynamic_cast(creator.pimpl())) - conn->printPendingPairs(os); + os << conn->pendingPairsPrinter(); os << std::endl; for (const auto& child : container.children()) { diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 2f7fcb1c..a3d3de9c 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -826,7 +826,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 } @@ -856,6 +856,7 @@ template bool ConnectingPrivate::hasPendingOpposites(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(); @@ -869,15 +870,16 @@ void ConnectingPrivate::compute() { static_cast(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; } From 9dfd6bd2eb043be5699e21a6fe901d7fe689fa6d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 25 Oct 2023 22:00:47 +0200 Subject: [PATCH 7/8] Fix duplicate solutions When adding pending state pairs for a new incoming state to Connect, we have to re-enable opposite states from ARMED state. This changes the order of states in the interface. If we do this while iterating over the states, we might add pairs multiple times, because iteration continues with same state at an earlier position. --- core/src/stage.cpp | 13 +++++++++++-- core/test/test_serial.cpp | 1 - 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index a3d3de9c..40dd413f 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -794,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(); bool have_enabled_opposites = false; + + // other interface states to re-enable (post-poned because otherwise order in other_interface changes during loop) + std::vector oit_to_enable; for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { if (!static_cast(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()>(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(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()>(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(nullptr, nullptr, &*it, InterfaceState::Status::ARMED); diff --git a/core/test/test_serial.cpp b/core/test/test_serial.cpp index 9d81cf3c..aefcba94 100644 --- a/core/test/test_serial.cpp +++ b/core/test/test_serial.cpp @@ -71,7 +71,6 @@ TEST_F(ConnectConnect, FailSucc) { } // https://github.com/ros-planning/moveit_task_constructor/issues/485#issuecomment-1760606116 -// TODO: Solutions are enumerated multiple times TEST_F(ConnectConnect, UniqueEnumeration) { add(t, new GeneratorMockup({ 1.0, 2.0, 3.0 })); auto con1 = add(t, new ConnectMockup()); From e163f57f9c264f40b10a861999a1132fbd356574 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 25 Oct 2023 22:59:14 +0200 Subject: [PATCH 8/8] Cleanup debug output --- core/src/stage.cpp | 2 +- .../motion_planning_tasks/src/remote_task_model.cpp | 12 +++++------- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 40dd413f..620b5a25 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -865,7 +865,7 @@ template bool ConnectingPrivate::hasPendingOpposites(const const InterfaceState* start) const; bool ConnectingPrivate::canCompute() const { - ROS_DEBUG_STREAM("canCompute " << name() << ": " << pendingPairsPrinter()); + // 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(); diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 8ab98047..430a3157 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -501,14 +501,12 @@ QVariant RemoteSolutionModel::data(const QModelIndex& index, int role) const { return item.id; case Qt::ToolTipRole: - switch (index.column()) { -#if 1 // show internal solution id in first column return item - case 0: - return item.id; - default: - return item.comment; +#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: