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);
|
void setName(const std::string& name);
|
||||||
|
|
||||||
uint32_t introspectionId() const;
|
uint32_t introspectionId() const;
|
||||||
|
Introspection* introspection() const;
|
||||||
|
|
||||||
/** set computation timeout (in seconds)
|
/** set computation timeout (in seconds)
|
||||||
*
|
*
|
||||||
|
|||||||
@ -301,10 +301,20 @@ private:
|
|||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(MonitoringGenerator)
|
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
|
class ConnectingPrivate : public ComputeBasePrivate
|
||||||
{
|
{
|
||||||
friend class Connecting;
|
friend class Connecting;
|
||||||
friend struct FallbacksPrivateConnect;
|
friend struct FallbacksPrivateConnect;
|
||||||
|
friend std::ostream& operator<<(std::ostream& os, const PendingPairsPrinter& printer);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
struct StatePair : std::pair<Interface::const_iterator, Interface::const_iterator>
|
struct StatePair : std::pair<Interface::const_iterator, Interface::const_iterator>
|
||||||
@ -337,7 +347,7 @@ public:
|
|||||||
template <Interface::Direction dir>
|
template <Interface::Direction dir>
|
||||||
bool hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const;
|
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:
|
private:
|
||||||
// Create a pair of Interface states for pending list, such that the order (start, end) is maintained
|
// 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;
|
ordered<StatePair> pending;
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(Connecting)
|
PIMPL_FUNCTIONS(Connecting)
|
||||||
|
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|||||||
@ -66,7 +66,7 @@ printChildrenInterfaces(const ContainerBasePrivate& container, bool success, con
|
|||||||
if (success)
|
if (success)
|
||||||
os << ++id << ' ';
|
os << ++id << ' ';
|
||||||
if (const auto conn = dynamic_cast<const ConnectingPrivate*>(creator.pimpl()))
|
if (const auto conn = dynamic_cast<const ConnectingPrivate*>(creator.pimpl()))
|
||||||
conn->printPendingPairs(os);
|
os << conn->pendingPairsPrinter();
|
||||||
os << std::endl;
|
os << std::endl;
|
||||||
|
|
||||||
for (const auto& child : container.children()) {
|
for (const auto& child : container.children()) {
|
||||||
@ -487,7 +487,8 @@ struct SolutionCollector
|
|||||||
solutions.emplace_back(std::make_pair(trace, prio));
|
solutions.emplace_back(std::make_pair(trace, prio));
|
||||||
} else {
|
} else {
|
||||||
for (SolutionBase* successor : next) {
|
for (SolutionBase* successor : next) {
|
||||||
assert(!successor->isFailure()); // We shouldn't have invalid solutions
|
if (successor->isFailure())
|
||||||
|
continue; // skip failures
|
||||||
trace.push_back(successor);
|
trace.push_back(successor);
|
||||||
traverse(*successor, prio + InterfaceState::Priority(1, successor->cost()));
|
traverse(*successor, prio + InterfaceState::Priority(1, successor->cost()));
|
||||||
trace.pop_back();
|
trace.pop_back();
|
||||||
|
|||||||
@ -55,6 +55,8 @@ bool isValidCharInName(char c); // unfortunately this is not declared in ros/na
|
|||||||
} // namespace names
|
} // namespace names
|
||||||
} // namespace ros
|
} // namespace ros
|
||||||
|
|
||||||
|
static const char* LOGGER = "introspection";
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
|
|
||||||
@ -206,6 +208,9 @@ uint32_t Introspection::stageId(const Stage* const s) const {
|
|||||||
|
|
||||||
uint32_t Introspection::solutionId(const SolutionBase& s) {
|
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));
|
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;
|
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.");
|
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);
|
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) {
|
void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest) {
|
||||||
const PropertyMap& src = source.properties();
|
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?
|
assert(it->priority().enabled()); // new solutions are feasible, aren't they?
|
||||||
InterfacePtr other_interface = pullInterface<dir>();
|
InterfacePtr other_interface = pullInterface<dir>();
|
||||||
bool have_enabled_opposites = false;
|
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) {
|
for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) {
|
||||||
if (!static_cast<Connecting*>(me_)->compatible(*it, *oit))
|
if (!static_cast<Connecting*>(me_)->compatible(*it, *oit))
|
||||||
continue;
|
continue;
|
||||||
|
|
||||||
// re-enable the opposing state oit (and its associated solution branch) if its status is ARMED
|
// 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
|
// https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202
|
||||||
if (oit->priority().status() == InterfaceState::Status::ARMED)
|
if (oit->priority().status() == InterfaceState::Status::ARMED) {
|
||||||
parent_pimpl->setStatus<opposite<dir>()>(me(), &*it, &*oit, InterfaceState::Status::ENABLED);
|
oit_to_enable.push_back(oit);
|
||||||
|
have_enabled_opposites = true;
|
||||||
|
}
|
||||||
if (oit->priority().enabled())
|
if (oit->priority().enabled())
|
||||||
have_enabled_opposites = true;
|
have_enabled_opposites = true;
|
||||||
|
|
||||||
// Remember all pending states, regardless of their status!
|
// Remember all pending states, regardless of their status!
|
||||||
pending.insert(make_pair<dir>(it, oit));
|
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
|
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
|
// pass creator=nullptr to skip hasPendingOpposites() check as we did this here already
|
||||||
parent_pimpl->setStatus<dir>(nullptr, nullptr, &*it, InterfaceState::Status::ARMED);
|
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 << d << " " << this->pullInterface(d) << ": " << *this->pullInterface(d) << std::endl;
|
||||||
}
|
}
|
||||||
os << std::setw(15) << " ";
|
os << std::setw(15) << " ";
|
||||||
printPendingPairs(os) << std::endl;
|
os << pendingPairsPrinter() << std::endl;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -853,6 +865,7 @@ template bool ConnectingPrivate::hasPendingOpposites<Interface::BACKWARD>(const
|
|||||||
const InterfaceState* start) const;
|
const InterfaceState* start) const;
|
||||||
|
|
||||||
bool ConnectingPrivate::canCompute() const {
|
bool ConnectingPrivate::canCompute() const {
|
||||||
|
// ROS_DEBUG_STREAM("canCompute " << name() << ": " << pendingPairsPrinter());
|
||||||
// Do we still have feasible pending state pairs?
|
// Do we still have feasible pending state pairs?
|
||||||
return !pending.empty() && pending.front().first->priority().enabled() &&
|
return !pending.empty() && pending.front().first->priority().enabled() &&
|
||||||
pending.front().second->priority().enabled();
|
pending.front().second->priority().enabled();
|
||||||
@ -866,15 +879,16 @@ void ConnectingPrivate::compute() {
|
|||||||
static_cast<Connecting*>(me_)->compute(from, to);
|
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);
|
const char* reset = InterfaceState::colorForStatus(3);
|
||||||
for (const auto& candidate : pending) {
|
for (const auto& candidate : impl->pending) {
|
||||||
size_t first = getIndex(*starts(), candidate.first);
|
size_t first = getIndex(*impl->starts(), candidate.first);
|
||||||
size_t second = getIndex(*ends(), candidate.second);
|
size_t second = getIndex(*impl->ends(), candidate.second);
|
||||||
os << InterfaceState::colorForStatus(candidate.first->priority().status()) << first << reset << ":"
|
os << InterfaceState::colorForStatus(candidate.first->priority().status()) << first << reset << ":"
|
||||||
<< InterfaceState::colorForStatus(candidate.second->priority().status()) << second << reset << " ";
|
<< InterfaceState::colorForStatus(candidate.second->priority().status()) << second << reset << " ";
|
||||||
}
|
}
|
||||||
if (pending.empty())
|
if (impl->pending.empty())
|
||||||
os << "---";
|
os << "---";
|
||||||
return os;
|
return os;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -34,9 +34,10 @@ if (CATKIN_ENABLE_TESTING)
|
|||||||
mtc_add_test("gmock" ${ARGN})
|
mtc_add_test("gmock" ${ARGN})
|
||||||
endmacro()
|
endmacro()
|
||||||
|
|
||||||
|
mtc_add_gmock(test_mockups.cpp)
|
||||||
mtc_add_gtest(test_stage.cpp)
|
mtc_add_gtest(test_stage.cpp)
|
||||||
mtc_add_gtest(test_container.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_pruning.cpp)
|
||||||
mtc_add_gtest(test_properties.cpp)
|
mtc_add_gtest(test_properties.cpp)
|
||||||
mtc_add_gtest(test_cost_terms.cpp)
|
mtc_add_gtest(test_cost_terms.cpp)
|
||||||
|
|||||||
@ -46,6 +46,19 @@ double PredefinedCosts::cost() const {
|
|||||||
return c;
|
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)
|
GeneratorMockup::GeneratorMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
|
||||||
: Generator{ "GEN" + std::to_string(++id_) }
|
: Generator{ "GEN" + std::to_string(++id_) }
|
||||||
, costs_{ std::move(costs) }
|
, costs_{ std::move(costs) }
|
||||||
@ -86,6 +99,7 @@ void ConnectMockup::compute(const InterfaceState& from, const InterfaceState& to
|
|||||||
|
|
||||||
auto solution{ std::make_shared<SubTrajectory>() };
|
auto solution{ std::make_shared<SubTrajectory>() };
|
||||||
solution->setCost(costs_.cost());
|
solution->setCost(costs_.cost());
|
||||||
|
solution->setComment(std::to_string(from.priority().cost()) + " -> " + std::to_string(to.priority().cost()));
|
||||||
connect(from, to, solution);
|
connect(from, to, solution);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -34,6 +34,18 @@ struct PredefinedCosts : CostTerm
|
|||||||
|
|
||||||
constexpr double INF{ std::numeric_limits<double>::infinity() };
|
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
|
struct GeneratorMockup : public Generator
|
||||||
{
|
{
|
||||||
planning_scene::PlanningScenePtr ps_;
|
planning_scene::PlanningScenePtr ps_;
|
||||||
@ -47,8 +59,8 @@ struct GeneratorMockup : public Generator
|
|||||||
// default to one solution to avoid infinity loops
|
// default to one solution to avoid infinity loops
|
||||||
GeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ std::list<double>{ 0.0 }, true },
|
GeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ std::list<double>{ 0.0 }, true },
|
||||||
std::size_t solutions_per_compute = 1);
|
std::size_t solutions_per_compute = 1);
|
||||||
GeneratorMockup(std::initializer_list<double> costs)
|
GeneratorMockup(std::initializer_list<double> costs, std::size_t solutions_per_compute = 1)
|
||||||
: GeneratorMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
|
: GeneratorMockup{ PredefinedCosts{ std::list<double>{ costs }, true }, solutions_per_compute } {}
|
||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
|
|||||||
@ -110,7 +110,7 @@ protected:
|
|||||||
Container container;
|
Container container;
|
||||||
InterfacePtr dummy;
|
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) {
|
void pushInterface(bool start = true, bool end = true) {
|
||||||
// pretend, that the container is connected
|
// 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 <list>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
|
|
||||||
#include <gtest/gtest.h>
|
|
||||||
|
|
||||||
#ifndef TYPED_TEST_SUITE
|
#ifndef TYPED_TEST_SUITE
|
||||||
#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES)
|
#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES)
|
||||||
#endif
|
#endif
|
||||||
@ -100,14 +98,7 @@ TEST_F(Pruning, ConnectConnectForward) {
|
|||||||
add(t, new GeneratorMockup({ 1, 2, 3 }));
|
add(t, new GeneratorMockup({ 1, 2, 3 }));
|
||||||
|
|
||||||
t.plan();
|
t.plan();
|
||||||
|
EXPECT_COSTS(t.solutions(), ::testing::ElementsAre(11, 12, 13, 21, 22, 23));
|
||||||
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_EQ(c1->runs_, 3u);
|
EXPECT_EQ(c1->runs_, 3u);
|
||||||
EXPECT_EQ(c2->runs_, 6u); // expect 6 instead of 9 calls
|
EXPECT_EQ(c2->runs_, 6u); // expect 6 instead of 9 calls
|
||||||
}
|
}
|
||||||
@ -123,13 +114,7 @@ TEST_F(Pruning, ConnectConnectBackward) {
|
|||||||
|
|
||||||
t.plan();
|
t.plan();
|
||||||
|
|
||||||
ASSERT_EQ(t.solutions().size(), 3u * 2u);
|
EXPECT_COSTS(t.solutions(), ::testing::ElementsAre(11, 12, 13, 21, 22, 23));
|
||||||
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_EQ(c1->runs_, 6u); // expect 6 instead of 9 calls
|
EXPECT_EQ(c1->runs_, 6u); // expect 6 instead of 9 calls
|
||||||
EXPECT_EQ(c2->runs_, 3u);
|
EXPECT_EQ(c2->runs_, 3u);
|
||||||
}
|
}
|
||||||
@ -207,3 +192,18 @@ TEST_F(Pruning, TwoConnects) {
|
|||||||
|
|
||||||
EXPECT_FALSE(t.plan());
|
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 "models.h"
|
||||||
#include <list>
|
#include <list>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
#include <gtest/gtest.h>
|
|
||||||
|
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
using namespace planning_scene;
|
using namespace planning_scene;
|
||||||
@ -56,13 +55,7 @@ TEST_F(ConnectConnect, SuccSucc) {
|
|||||||
add(t, new GeneratorMockup({ 0.0 }));
|
add(t, new GeneratorMockup({ 0.0 }));
|
||||||
|
|
||||||
EXPECT_TRUE(t.plan());
|
EXPECT_TRUE(t.plan());
|
||||||
ASSERT_EQ(t.solutions().size(), 3u * 2u);
|
EXPECT_COSTS(t.solutions(), ::testing::ElementsAre(11, 12, 13, 21, 22, 23));
|
||||||
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;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// https://github.com/ros-planning/moveit_task_constructor/issues/218
|
// https://github.com/ros-planning/moveit_task_constructor/issues/218
|
||||||
@ -76,3 +69,17 @@ TEST_F(ConnectConnect, FailSucc) {
|
|||||||
|
|
||||||
EXPECT_FALSE(t.plan());
|
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;
|
return item.id;
|
||||||
|
|
||||||
case Qt::ToolTipRole:
|
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;
|
return item.comment;
|
||||||
|
break;
|
||||||
|
|
||||||
case Qt::DisplayRole:
|
case Qt::DisplayRole:
|
||||||
switch (index.column()) {
|
switch (index.column()) {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user