mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
commit
7638e5ff8b
@ -184,6 +184,7 @@ public:
|
||||
void setName(const std::string& name);
|
||||
|
||||
uint32_t introspectionId() const;
|
||||
Introspection* introspection() const;
|
||||
|
||||
/** set computation timeout (in seconds)
|
||||
*
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
76
core/test/test_mockups.cpp
Normal file
76
core/test/test_mockups.cpp
Normal 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
|
||||
@ -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
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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()) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user