mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge different mockup implementations
Co-authored-by: Jascha Kühn <57101356+j-kuehn@users.noreply.github.com>
This commit is contained in:
parent
4b1f240c21
commit
74d33c4ec0
@ -6,7 +6,7 @@
|
||||
if (CATKIN_ENABLE_TESTING)
|
||||
find_package(rostest REQUIRED)
|
||||
|
||||
add_library(gtest_utils gtest_value_printers.cpp models.cpp)
|
||||
add_library(gtest_utils gtest_value_printers.cpp models.cpp stage_mockups.cpp)
|
||||
target_link_libraries(gtest_utils ${PROJECT_NAME})
|
||||
|
||||
catkin_add_gtest(${PROJECT_NAME}-test-stage test_stage.cpp)
|
||||
|
||||
125
core/test/stage_mockups.cpp
Normal file
125
core/test/stage_mockups.cpp
Normal file
@ -0,0 +1,125 @@
|
||||
#include <moveit/task_constructor/stage_p.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
#include "stage_mockups.h"
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
|
||||
unsigned int GeneratorMockup::id_ = 0;
|
||||
unsigned int MonitoringGeneratorMockup::id_ = 0;
|
||||
unsigned int ConnectMockup::id_ = 0;
|
||||
unsigned int PropagatorMockup::id_ = 0;
|
||||
unsigned int ForwardMockup::id_ = 0;
|
||||
unsigned int BackwardMockup::id_ = 0;
|
||||
|
||||
void resetMockupIds() {
|
||||
GeneratorMockup::id_ = 0;
|
||||
MonitoringGeneratorMockup::id_ = 0;
|
||||
ConnectMockup::id_ = 0;
|
||||
PropagatorMockup::id_ = 0;
|
||||
ForwardMockup::id_ = 0;
|
||||
BackwardMockup::id_ = 0;
|
||||
}
|
||||
|
||||
PredefinedCosts::PredefinedCosts(std::list<double>&& costs, bool finite)
|
||||
: costs_{ std::move(costs) }, finite_{ finite } {}
|
||||
|
||||
bool PredefinedCosts::exhausted() const {
|
||||
return costs_.empty();
|
||||
}
|
||||
|
||||
double PredefinedCosts::cost() const {
|
||||
// keep the tests going, but this should not happen
|
||||
EXPECT_GT(costs_.size(), 0u);
|
||||
if (costs_.empty())
|
||||
return 0.0;
|
||||
|
||||
double c{ costs_.front() };
|
||||
|
||||
if (finite_ || costs_.size() > 1) {
|
||||
costs_.pop_front();
|
||||
}
|
||||
|
||||
return c;
|
||||
}
|
||||
|
||||
GeneratorMockup::GeneratorMockup(PredefinedCosts&& costs)
|
||||
: Generator{ "GEN" + std::to_string(++id_) }, costs_{ std::move(costs) } {}
|
||||
|
||||
void GeneratorMockup::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
||||
ps_.reset(new planning_scene::PlanningScene(robot_model));
|
||||
Generator::init(robot_model);
|
||||
}
|
||||
|
||||
bool GeneratorMockup::canCompute() const {
|
||||
// check if runs are being used and if there are still runs left (costs are then never exhausted) or if costs
|
||||
// are being used and they are not exhausted yet
|
||||
return !costs_.exhausted();
|
||||
}
|
||||
|
||||
void GeneratorMockup::compute() {
|
||||
++runs_;
|
||||
|
||||
spawn(InterfaceState(ps_), costs_.cost());
|
||||
}
|
||||
|
||||
MonitoringGeneratorMockup::MonitoringGeneratorMockup(Stage* monitored, PredefinedCosts&& costs)
|
||||
: MonitoringGenerator{ "MON" + std::to_string(++id_), monitored }, costs_{ std::move(costs) } {}
|
||||
|
||||
void MonitoringGeneratorMockup::onNewSolution(const SolutionBase& s) {
|
||||
++runs_;
|
||||
|
||||
spawn(InterfaceState{ s.end()->scene()->diff() }, SubTrajectory{});
|
||||
}
|
||||
|
||||
ConnectMockup::ConnectMockup(PredefinedCosts&& costs)
|
||||
: Connecting{ "CON" + std::to_string(++id_) }, costs_{ std::move(costs) } {}
|
||||
|
||||
void ConnectMockup::compute(const InterfaceState& from, const InterfaceState& to) {
|
||||
++runs_;
|
||||
|
||||
auto solution{ std::make_shared<SubTrajectory>() };
|
||||
solution->setCost(costs_.cost());
|
||||
connect(from, to, solution);
|
||||
}
|
||||
|
||||
PropagatorMockup::PropagatorMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
|
||||
: PropagatingEitherWay{ "PRO" + std::to_string(++id_) }
|
||||
, costs_{ std::move(costs) }
|
||||
, solutions_per_compute_{ solutions_per_compute } {}
|
||||
|
||||
void PropagatorMockup::computeForward(const InterfaceState& from) {
|
||||
++runs_;
|
||||
|
||||
for (std::size_t i = 0; i < solutions_per_compute_; ++i) {
|
||||
SubTrajectory solution{ robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost() };
|
||||
sendForward(from, InterfaceState{ from.scene()->diff() }, std::move(solution));
|
||||
}
|
||||
}
|
||||
|
||||
void PropagatorMockup::computeBackward(const InterfaceState& to) {
|
||||
++runs_;
|
||||
|
||||
for (std::size_t i = 0; i < solutions_per_compute_; ++i) {
|
||||
SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost());
|
||||
sendBackward(InterfaceState(to.scene()->diff()), to, std::move(solution));
|
||||
}
|
||||
}
|
||||
|
||||
ForwardMockup::ForwardMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
|
||||
: PropagatorMockup{ std::move(costs), solutions_per_compute } {
|
||||
restrictDirection(FORWARD);
|
||||
setName("FWD" + std::to_string(++id_));
|
||||
}
|
||||
|
||||
BackwardMockup::BackwardMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
|
||||
: PropagatorMockup{ std::move(costs), solutions_per_compute } {
|
||||
restrictDirection(BACKWARD);
|
||||
setName("BWD" + std::to_string(++id_));
|
||||
}
|
||||
|
||||
} // namespace task_constructor
|
||||
} // namespace moveit
|
||||
117
core/test/stage_mockups.h
Normal file
117
core/test/stage_mockups.h
Normal file
@ -0,0 +1,117 @@
|
||||
#pragma once
|
||||
|
||||
#include <moveit/task_constructor/stage_p.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
|
||||
MOVEIT_STRUCT_FORWARD(PredefinedCosts);
|
||||
struct PredefinedCosts : CostTerm
|
||||
{
|
||||
mutable std::list<double> costs_; // list of costs to assign
|
||||
mutable bool finite_;
|
||||
|
||||
PredefinedCosts(std::list<double>&& costs, bool finite = true);
|
||||
|
||||
static PredefinedCosts constant(double c) { return PredefinedCosts{ std::list<double>{ c }, false }; }
|
||||
static PredefinedCosts single(double c) { return PredefinedCosts{ std::list<double>{ c }, true }; }
|
||||
|
||||
bool exhausted() const;
|
||||
double cost() const;
|
||||
|
||||
double operator()(const SubTrajectory& /*s*/, std::string& /*comment*/) const override { return cost(); }
|
||||
double operator()(const SolutionSequence& /*s*/, std::string& /*comment*/) const override { return cost(); }
|
||||
double operator()(const WrappedSolution& /*s*/, std::string& /*comment*/) const override { return cost(); }
|
||||
};
|
||||
|
||||
struct GeneratorMockup : public Generator
|
||||
{
|
||||
planning_scene::PlanningScenePtr ps_;
|
||||
|
||||
PredefinedCosts costs_;
|
||||
size_t runs_{ 0 };
|
||||
|
||||
static unsigned int id_;
|
||||
|
||||
// default to one solution to avoid infinity loops
|
||||
GeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ std::list<double>{ 0.0 }, true });
|
||||
GeneratorMockup(std::initializer_list<double> costs)
|
||||
: GeneratorMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
bool canCompute() const override;
|
||||
void compute() override;
|
||||
};
|
||||
|
||||
struct MonitoringGeneratorMockup : public MonitoringGenerator
|
||||
{
|
||||
PredefinedCosts costs_;
|
||||
size_t runs_{ 0 };
|
||||
|
||||
static unsigned int id_;
|
||||
|
||||
MonitoringGeneratorMockup(Stage* monitored, PredefinedCosts&& costs = PredefinedCosts::constant(0.0));
|
||||
MonitoringGeneratorMockup(Stage* monitored, std::initializer_list<double> costs)
|
||||
: MonitoringGeneratorMockup{ monitored, PredefinedCosts{ std::list<double>{ costs }, true } } {}
|
||||
|
||||
bool canCompute() const override { return false; }
|
||||
void compute() override {}
|
||||
void onNewSolution(const SolutionBase& s) override;
|
||||
};
|
||||
|
||||
struct ConnectMockup : public Connecting
|
||||
{
|
||||
PredefinedCosts costs_;
|
||||
size_t runs_{ 0 };
|
||||
|
||||
static unsigned int id_;
|
||||
|
||||
ConnectMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0));
|
||||
ConnectMockup(std::initializer_list<double> costs)
|
||||
: ConnectMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
|
||||
|
||||
using Connecting::compatible; // make this accessible for testing
|
||||
|
||||
void compute(const InterfaceState& from, const InterfaceState& to) override;
|
||||
};
|
||||
|
||||
struct PropagatorMockup : public PropagatingEitherWay
|
||||
{
|
||||
PredefinedCosts costs_;
|
||||
size_t runs_{ 0 };
|
||||
std::size_t solutions_per_compute_;
|
||||
|
||||
static unsigned int id_;
|
||||
|
||||
PropagatorMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0), std::size_t solutions_per_compute = 1);
|
||||
PropagatorMockup(std::initializer_list<double> costs)
|
||||
: PropagatorMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
|
||||
|
||||
void computeForward(const InterfaceState& from) override;
|
||||
void computeBackward(const InterfaceState& to) override;
|
||||
};
|
||||
|
||||
struct ForwardMockup : public PropagatorMockup
|
||||
{
|
||||
static unsigned int id_;
|
||||
|
||||
ForwardMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0), std::size_t solutions_per_compute = 1);
|
||||
ForwardMockup(std::initializer_list<double> costs)
|
||||
: ForwardMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
|
||||
};
|
||||
|
||||
struct BackwardMockup : public PropagatorMockup
|
||||
{
|
||||
static unsigned int id_;
|
||||
|
||||
BackwardMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0), std::size_t solutions_per_compute = 1);
|
||||
BackwardMockup(std::initializer_list<double> costs)
|
||||
: BackwardMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
|
||||
};
|
||||
|
||||
// reset ids of all Mockup types (used to generate unique stage names)
|
||||
void resetMockupIds();
|
||||
|
||||
} // namespace task_constructor
|
||||
} // namespace moveit
|
||||
@ -4,8 +4,10 @@
|
||||
#include <moveit/task_constructor/stages/fixed_state.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
#include "stage_mockups.h"
|
||||
#include "models.h"
|
||||
#include "gtest_value_printers.h"
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <initializer_list>
|
||||
#include <chrono>
|
||||
@ -13,101 +15,6 @@
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
static unsigned int MOCK_ID = 0;
|
||||
|
||||
class GeneratorMockup : public Generator
|
||||
{
|
||||
moveit::core::RobotModelConstPtr robot;
|
||||
int runs = 0;
|
||||
|
||||
public:
|
||||
GeneratorMockup(int runs = 0) : Generator("generator " + std::to_string(++MOCK_ID)), runs(runs) {}
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override { robot = robot_model; }
|
||||
bool canCompute() const override { return runs > 0; }
|
||||
void compute() override {
|
||||
if (runs > 0) {
|
||||
--runs;
|
||||
spawn(InterfaceState(std::make_shared<planning_scene::PlanningScene>(robot)), SubTrajectory());
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
class MonitoringGeneratorMockup : public MonitoringGenerator
|
||||
{
|
||||
public:
|
||||
MonitoringGeneratorMockup(Stage* monitored)
|
||||
: MonitoringGenerator("monitoring generator " + std::to_string(++MOCK_ID), monitored) {}
|
||||
bool canCompute() const override { return false; }
|
||||
void compute() override {}
|
||||
void onNewSolution(const SolutionBase& s) override {
|
||||
spawn(InterfaceState(s.end()->scene()->diff()), SubTrajectory());
|
||||
}
|
||||
};
|
||||
|
||||
class PropagatorMockup : public PropagatingEitherWay
|
||||
{
|
||||
int fw_runs = 0;
|
||||
int bw_runs = 0;
|
||||
|
||||
public:
|
||||
PropagatorMockup(int fw = 0, int bw = 0)
|
||||
: PropagatingEitherWay("propagate " + std::to_string(++MOCK_ID)), fw_runs(fw), bw_runs(bw) {}
|
||||
void computeForward(const InterfaceState& from) override {
|
||||
if (fw_runs > 0) {
|
||||
--fw_runs;
|
||||
sendForward(from, InterfaceState(from.scene()->diff()), SubTrajectory());
|
||||
}
|
||||
}
|
||||
void computeBackward(const InterfaceState& to) override {
|
||||
if (bw_runs > 0) {
|
||||
--bw_runs;
|
||||
sendBackward(InterfaceState(to.scene()->diff()), to, SubTrajectory());
|
||||
}
|
||||
}
|
||||
};
|
||||
class ForwardMockup : public PropagatorMockup
|
||||
{
|
||||
public:
|
||||
ForwardMockup(int runs = 0) : PropagatorMockup(runs, 0) {
|
||||
restrictDirection(FORWARD);
|
||||
setName("forward " + std::to_string(MOCK_ID));
|
||||
}
|
||||
};
|
||||
class BackwardMockup : public PropagatorMockup
|
||||
{
|
||||
public:
|
||||
BackwardMockup(int runs = 0) : PropagatorMockup(0, runs) {
|
||||
restrictDirection(BACKWARD);
|
||||
setName("backward " + std::to_string(MOCK_ID));
|
||||
}
|
||||
};
|
||||
|
||||
// ForwardMockup that takes a while for its computation
|
||||
class TimedForwardMockup : public ForwardMockup
|
||||
{
|
||||
std::chrono::milliseconds duration_;
|
||||
|
||||
public:
|
||||
TimedForwardMockup(std::chrono::milliseconds duration) : ForwardMockup(1000), duration_(duration) {}
|
||||
void computeForward(const InterfaceState& from) override {
|
||||
std::this_thread::sleep_for(duration_);
|
||||
ForwardMockup::computeForward(from);
|
||||
}
|
||||
};
|
||||
|
||||
class ConnectMockup : public Connecting
|
||||
{
|
||||
int runs = 0;
|
||||
|
||||
public:
|
||||
ConnectMockup(int runs = 0) : Connecting("connect " + std::to_string(++MOCK_ID)), runs(runs) {}
|
||||
void compute(const InterfaceState& from, const InterfaceState& to) override {
|
||||
if (runs > 0)
|
||||
--runs;
|
||||
connect(from, to, std::make_shared<SubTrajectory>());
|
||||
}
|
||||
};
|
||||
|
||||
enum StageType
|
||||
{
|
||||
GEN,
|
||||
@ -116,23 +23,23 @@ enum StageType
|
||||
ANY,
|
||||
CONN
|
||||
};
|
||||
void append(ContainerBase& c, const std::initializer_list<StageType>& types, int runs = 0) {
|
||||
void append(ContainerBase& c, const std::initializer_list<StageType>& types) {
|
||||
for (StageType t : types) {
|
||||
switch (t) {
|
||||
case GEN:
|
||||
c.add(std::make_unique<GeneratorMockup>(runs));
|
||||
c.add(std::make_unique<GeneratorMockup>());
|
||||
break;
|
||||
case FW:
|
||||
c.add(std::make_unique<ForwardMockup>(runs));
|
||||
c.add(std::make_unique<ForwardMockup>());
|
||||
break;
|
||||
case BW:
|
||||
c.add(std::make_unique<BackwardMockup>(runs));
|
||||
c.add(std::make_unique<BackwardMockup>());
|
||||
break;
|
||||
case ANY:
|
||||
c.add(std::make_unique<PropagatorMockup>(runs, runs));
|
||||
c.add(std::make_unique<PropagatorMockup>());
|
||||
break;
|
||||
case CONN:
|
||||
c.add(std::make_unique<ConnectMockup>(runs));
|
||||
c.add(std::make_unique<ConnectMockup>());
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -215,7 +122,7 @@ protected:
|
||||
}
|
||||
void initContainer(const std::initializer_list<StageType>& types = {}) {
|
||||
container.clear();
|
||||
MOCK_ID = 0;
|
||||
resetMockupIds();
|
||||
append(container, types);
|
||||
}
|
||||
|
||||
@ -505,7 +412,7 @@ TEST_F(SerialTest, interface_detection) {
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
Stage::pointer make(const std::string& name, const std::initializer_list<StageType>& children, int runs = 0) {
|
||||
Stage::pointer make(const std::string& name, const std::initializer_list<StageType>& children) {
|
||||
auto container = new T(name);
|
||||
append(*container, children);
|
||||
return Stage::pointer(container);
|
||||
@ -669,13 +576,13 @@ TEST_F(ParallelTest, init_any) {
|
||||
}
|
||||
|
||||
TEST(Task, move) {
|
||||
MOCK_ID = 0;
|
||||
resetMockupIds();
|
||||
Task t1("foo");
|
||||
t1.add(std::make_unique<GeneratorMockup>());
|
||||
t1.add(std::make_unique<GeneratorMockup>());
|
||||
EXPECT_EQ(t1.stages()->numChildren(), 2u);
|
||||
|
||||
MOCK_ID = 0;
|
||||
resetMockupIds();
|
||||
Task t2 = std::move(t1);
|
||||
EXPECT_EQ(t2.stages()->numChildren(), 2u);
|
||||
EXPECT_EQ(t1.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move)
|
||||
@ -692,13 +599,13 @@ TEST(Task, reuse) {
|
||||
t.setRobotModel(robot_model);
|
||||
|
||||
auto configure = [](Task& t) {
|
||||
MOCK_ID = 0;
|
||||
resetMockupIds();
|
||||
auto ref = new stages::FixedState("fixed");
|
||||
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
|
||||
ref->setState(scene);
|
||||
|
||||
t.add(Stage::pointer(ref));
|
||||
t.add(std::make_unique<ConnectMockup>(2));
|
||||
t.add(std::make_unique<ConnectMockup>());
|
||||
t.add(std::make_unique<MonitoringGeneratorMockup>(ref));
|
||||
};
|
||||
|
||||
@ -719,13 +626,26 @@ TEST(Task, reuse) {
|
||||
}
|
||||
}
|
||||
|
||||
// ForwardMockup that takes a while for its computation
|
||||
class TimedForwardMockup : public ForwardMockup
|
||||
{
|
||||
std::chrono::milliseconds duration_;
|
||||
|
||||
public:
|
||||
TimedForwardMockup(std::chrono::milliseconds duration) : ForwardMockup{}, duration_{ duration } {}
|
||||
void computeForward(const InterfaceState& from) override {
|
||||
std::this_thread::sleep_for(duration_);
|
||||
ForwardMockup::computeForward(from);
|
||||
}
|
||||
};
|
||||
|
||||
TEST(Task, timeout) {
|
||||
MOCK_ID = 0;
|
||||
resetMockupIds();
|
||||
Task t;
|
||||
t.setRobotModel(getModel());
|
||||
|
||||
auto timeout = std::chrono::milliseconds(10);
|
||||
t.add(std::make_unique<GeneratorMockup>(100)); // allow up to 100 solutions spawned
|
||||
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::constant(0.0)));
|
||||
t.add(std::make_unique<TimedForwardMockup>(timeout));
|
||||
|
||||
// no timeout, but limited number of solutions
|
||||
@ -751,15 +671,15 @@ TEST(Task, timeout) {
|
||||
}
|
||||
|
||||
TEST(Fallback, failing) {
|
||||
MOCK_ID = 0;
|
||||
resetMockupIds();
|
||||
Task t;
|
||||
t.setRobotModel(getModel());
|
||||
|
||||
t.add(std::make_unique<GeneratorMockup>());
|
||||
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(0.0)));
|
||||
|
||||
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
|
||||
fallback->add(std::make_unique<ForwardMockup>());
|
||||
fallback->add(std::make_unique<ForwardMockup>());
|
||||
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0), 0));
|
||||
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0), 0));
|
||||
t.add(std::move(fallback));
|
||||
|
||||
EXPECT_FALSE(t.plan());
|
||||
|
||||
@ -11,88 +11,58 @@
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "stage_mockups.h"
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
using namespace planning_scene;
|
||||
|
||||
const double STAGE_COST{ 7.0 };
|
||||
constexpr double STAGE_COST{ 7.0 };
|
||||
constexpr double TERM_COST{ 1.0 };
|
||||
|
||||
const double TRAJECTORY_DURATION{ 9.0 };
|
||||
|
||||
class GeneratorMockup : public Generator
|
||||
struct GeneratorCostMockup : public GeneratorMockup
|
||||
{
|
||||
PlanningScenePtr ps;
|
||||
InterfacePtr prev;
|
||||
InterfacePtr next;
|
||||
|
||||
public:
|
||||
GeneratorMockup() : Generator("generator") {
|
||||
prev.reset(new Interface);
|
||||
next.reset(new Interface);
|
||||
pimpl()->setPrevEnds(prev);
|
||||
pimpl()->setNextStarts(next);
|
||||
}
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
|
||||
ps.reset(new PlanningScene(robot_model));
|
||||
Generator::init(robot_model);
|
||||
}
|
||||
|
||||
bool canCompute() const override { return true; }
|
||||
|
||||
void compute() override {
|
||||
InterfaceState state(ps);
|
||||
spawn(std::move(state), STAGE_COST);
|
||||
}
|
||||
GeneratorCostMockup() : GeneratorMockup{ PredefinedCosts{ { STAGE_COST }, true } } {}
|
||||
};
|
||||
|
||||
class ConnectMockup : public Connecting
|
||||
struct ConnectCostMockup : public ConnectMockup
|
||||
{
|
||||
using Connecting::Connecting;
|
||||
|
||||
void compute(const InterfaceState& from, const InterfaceState& to) override {
|
||||
auto solution{ std::make_shared<SubTrajectory>() };
|
||||
solution->setCost(STAGE_COST);
|
||||
connect(from, to, solution);
|
||||
}
|
||||
ConnectCostMockup() : ConnectMockup{ PredefinedCosts::constant(STAGE_COST) } {}
|
||||
};
|
||||
|
||||
class ForwardMockup : public PropagatingForward
|
||||
struct ForwardCostMockup : public ForwardMockup
|
||||
{
|
||||
PlanningScenePtr ps;
|
||||
ForwardCostMockup() : ForwardMockup{ PredefinedCosts::constant(STAGE_COST) } {}
|
||||
};
|
||||
|
||||
public:
|
||||
using PropagatingForward::PropagatingForward;
|
||||
struct BackwardCostMockup : public BackwardMockup
|
||||
{
|
||||
BackwardCostMockup() : BackwardMockup{ PredefinedCosts::constant(STAGE_COST) } {}
|
||||
};
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
|
||||
ps.reset(new PlanningScene(robot_model));
|
||||
PropagatingForward::init(robot_model);
|
||||
}
|
||||
struct ForwardTrajectoryMockup : public ForwardMockup
|
||||
{
|
||||
ForwardTrajectoryMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0),
|
||||
std::size_t solutions_per_compute = 1)
|
||||
: ForwardMockup{ std::move(costs), solutions_per_compute } {}
|
||||
|
||||
void computeForward(const InterfaceState& from) override {
|
||||
SubTrajectory solution;
|
||||
auto traj{ std::make_shared<robot_trajectory::RobotTrajectory>(ps->getRobotModel(),
|
||||
ps->getRobotModel()->getJointModelGroups()[0]) };
|
||||
traj->addSuffixWayPoint(ps->getCurrentState(), 0.0);
|
||||
traj->addSuffixWayPoint(ps->getCurrentState(), TRAJECTORY_DURATION);
|
||||
solution.setTrajectory(traj);
|
||||
solution.setCost(STAGE_COST);
|
||||
InterfaceState to(from);
|
||||
++runs_;
|
||||
|
||||
sendForward(from, std::move(to), std::move(solution));
|
||||
};
|
||||
};
|
||||
for (size_t i = 0; i < solutions_per_compute_; ++i) {
|
||||
SubTrajectory solution;
|
||||
auto traj{ std::make_shared<robot_trajectory::RobotTrajectory>(from.scene()->getRobotModel(), nullptr) };
|
||||
planning_scene::PlanningScenePtr ps{ from.scene()->diff() };
|
||||
traj->addSuffixWayPoint(ps->getCurrentState(), 0.0);
|
||||
traj->addSuffixWayPoint(ps->getCurrentState(), TRAJECTORY_DURATION);
|
||||
solution.setTrajectory(traj);
|
||||
solution.setCost(STAGE_COST);
|
||||
InterfaceState to(from);
|
||||
|
||||
class BackwardMockup : public PropagatingBackward
|
||||
{
|
||||
using PropagatingBackward::PropagatingBackward;
|
||||
|
||||
void computeBackward(const InterfaceState& to) override {
|
||||
SubTrajectory solution;
|
||||
solution.setCost(STAGE_COST);
|
||||
InterfaceState from(to);
|
||||
|
||||
sendBackward(std::move(from), to, std::move(solution));
|
||||
};
|
||||
sendForward(from, std::move(to), std::move(solution));
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
@ -105,7 +75,9 @@ public:
|
||||
InterfaceStatePtr state_start, state_end;
|
||||
|
||||
Standalone(const moveit::core::RobotModelConstPtr& robot)
|
||||
: T(), robot(robot), dummy(std::make_shared<Interface>()), ps(new planning_scene::PlanningScene(robot)) {}
|
||||
: T(), robot(robot), dummy(std::make_shared<Interface>()), ps(new planning_scene::PlanningScene(robot)) {
|
||||
resetMockupIds();
|
||||
}
|
||||
|
||||
// reset and prepare for a compute step
|
||||
void prepare() {
|
||||
@ -173,7 +145,7 @@ TEST(CostTerm, SolutionConnected) {
|
||||
const moveit::core::RobotModelConstPtr robot{ getModel() };
|
||||
|
||||
Standalone<SerialContainer> container(robot);
|
||||
auto stage{ std::make_unique<ConnectMockup>() };
|
||||
auto stage{ std::make_unique<ConnectCostMockup>() };
|
||||
|
||||
// custom CostTerm to verify SubTrajectory is hooked up to its states & creator
|
||||
class VerifySolutionCostTerm : public TrajectoryCostTerm
|
||||
@ -193,38 +165,40 @@ TEST(CostTerm, SolutionConnected) {
|
||||
const_cast<const SerialContainerPrivate*>(container_.pimpl())->internalToExternalMap().at(s.end()))
|
||||
<< "SubTrajectory is not connected to its expected end InterfaceState";
|
||||
EXPECT_EQ(s.creator(), creator_);
|
||||
return 1.0;
|
||||
return TERM_COST;
|
||||
}
|
||||
};
|
||||
|
||||
stage->setCostTerm(std::make_unique<VerifySolutionCostTerm>(container, &*stage));
|
||||
container.computeWithStages({ std::move(stage) });
|
||||
EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "custom CostTerm overwrites stage cost";
|
||||
EXPECT_EQ(container.solutions().front()->cost(), TERM_COST) << "custom CostTerm overwrites stage cost";
|
||||
}
|
||||
|
||||
TEST(CostTerm, SetLambdaCostTerm) {
|
||||
const moveit::core::RobotModelConstPtr robot{ getModel() };
|
||||
|
||||
Standalone<SerialContainer> container(robot);
|
||||
auto stage{ std::make_unique<GeneratorMockup>() };
|
||||
stage->setCostTerm([](auto&& /*s*/) { return 1.0; });
|
||||
container.computeWithStages({ std::move(stage) });
|
||||
EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "can use simple lambda signature";
|
||||
|
||||
stage = std::make_unique<GeneratorMockup>();
|
||||
auto stage{ std::make_unique<GeneratorCostMockup>() };
|
||||
stage->setCostTerm([](auto&& /*s*/) { return TERM_COST; });
|
||||
container.computeWithStages({ std::move(stage) });
|
||||
EXPECT_EQ(container.solutions().front()->cost(), TERM_COST) << "can use simple lambda signature";
|
||||
|
||||
stage = std::make_unique<GeneratorCostMockup>();
|
||||
stage->setCostTerm([](auto&& /*s*/, auto&& /*comment*/) { return 1.0; });
|
||||
container.computeWithStages({ std::move(stage) });
|
||||
EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "can use full lambda signature";
|
||||
|
||||
stage = std::make_unique<GeneratorMockup>();
|
||||
stage->setCostTerm([](auto&& /*s*/, auto&& comment) {
|
||||
comment = "I want the user to see this";
|
||||
const std::string cost_term_comment{ "I want the user to see this" };
|
||||
stage = std::make_unique<GeneratorCostMockup>();
|
||||
stage->setCostTerm([&](auto&& /*s*/, auto&& comment) {
|
||||
comment = cost_term_comment;
|
||||
return 1.0;
|
||||
});
|
||||
container.computeWithStages({ std::move(stage) });
|
||||
auto sol = std::dynamic_pointer_cast<const SolutionSequence>(container.solutions().front());
|
||||
EXPECT_EQ(sol->cost(), 1.0);
|
||||
EXPECT_EQ(sol->solutions()[0]->comment(), "I want the user to see this") << "can write to comment";
|
||||
EXPECT_EQ(sol->solutions()[0]->comment(), cost_term_comment) << "can write to comment";
|
||||
}
|
||||
|
||||
TEST(CostTerm, CostOverwrite) {
|
||||
@ -232,14 +206,14 @@ TEST(CostTerm, CostOverwrite) {
|
||||
|
||||
Standalone<SerialContainer> container(robot);
|
||||
|
||||
container.computeWithStages({ std::make_unique<GeneratorMockup>() });
|
||||
container.computeWithStages({ std::make_unique<GeneratorCostMockup>() });
|
||||
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "return cost of stage by default";
|
||||
|
||||
container.computeWithStageCost({ std::make_unique<GeneratorMockup>() }, nullptr);
|
||||
container.computeWithStageCost({ std::make_unique<GeneratorCostMockup>() }, nullptr);
|
||||
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost";
|
||||
|
||||
auto constant_cost{ std::make_shared<cost::Constant>(1.0) };
|
||||
container.computeWithStageCost({ std::make_unique<GeneratorMockup>() }, constant_cost);
|
||||
container.computeWithStageCost({ std::make_unique<GeneratorCostMockup>() }, constant_cost);
|
||||
EXPECT_EQ(container.solutions().front()->cost(), constant_cost->cost) << "custom cost overwrites stage cost";
|
||||
}
|
||||
|
||||
@ -251,16 +225,16 @@ TEST(CostTerm, StageTypes) {
|
||||
auto constant{ std::make_shared<cost::Constant>(1.0) };
|
||||
|
||||
// already tested above
|
||||
// cont.computeWithStageCost(std::make_unique<GeneratorMockup>(), constant);
|
||||
// cont.computeWithStageCost(std::make_unique<GeneratorMockupCost>(), constant);
|
||||
// EXPECT_EQ(cont.solutions().front()->cost(), constant.cost);
|
||||
|
||||
container.computeWithStageCost({ std::make_unique<ConnectMockup>() }, constant);
|
||||
container.computeWithStageCost({ std::make_unique<ConnectCostMockup>() }, constant);
|
||||
EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for connect";
|
||||
|
||||
container.computeWithStageCost({ std::make_unique<ForwardMockup>() }, constant);
|
||||
container.computeWithStageCost({ std::make_unique<ForwardCostMockup>() }, constant);
|
||||
EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for forward propagator";
|
||||
|
||||
container.computeWithStageCost({ std::make_unique<BackwardMockup>() }, constant);
|
||||
container.computeWithStageCost({ std::make_unique<BackwardCostMockup>() }, constant);
|
||||
EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for backward propagator";
|
||||
}
|
||||
|
||||
@ -268,7 +242,7 @@ TEST(CostTerm, PassThroughUsesCost) {
|
||||
moveit::core::RobotModelPtr robot{ getModel() };
|
||||
Standalone<stages::PassThrough> container(robot);
|
||||
|
||||
auto stage{ std::make_unique<BackwardMockup>() };
|
||||
auto stage{ std::make_unique<BackwardCostMockup>() };
|
||||
auto constant{ std::make_shared<cost::Constant>(84.0) };
|
||||
stage->setCostTerm(constant);
|
||||
|
||||
@ -283,7 +257,7 @@ TEST(CostTerm, PassThroughOverwritesCost) {
|
||||
moveit::core::RobotModelPtr robot{ getModel() };
|
||||
Standalone<stages::PassThrough> container(robot);
|
||||
|
||||
auto stage{ std::make_unique<BackwardMockup>() };
|
||||
auto stage{ std::make_unique<BackwardCostMockup>() };
|
||||
auto constant_inner{ std::make_shared<cost::Constant>(84.0) };
|
||||
stage->setCostTerm(constant_inner);
|
||||
|
||||
@ -300,7 +274,7 @@ TEST(CostTerm, PassThroughCanModifyCost) {
|
||||
moveit::core::RobotModelPtr robot{ getModel() };
|
||||
Standalone<stages::PassThrough> container(robot);
|
||||
|
||||
auto stage{ std::make_unique<BackwardMockup>() };
|
||||
auto stage{ std::make_unique<BackwardCostMockup>() };
|
||||
auto constant{ std::make_shared<cost::Constant>(8.0) };
|
||||
stage->setCostTerm(constant);
|
||||
container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); });
|
||||
@ -315,18 +289,18 @@ TEST(CostTerm, CompositeSolutions) {
|
||||
Standalone<SerialContainer> container{ getModel() };
|
||||
|
||||
{
|
||||
auto s1{ std::make_unique<ForwardMockup>() };
|
||||
auto s2{ std::make_unique<ForwardMockup>() };
|
||||
auto s1{ std::make_unique<ForwardCostMockup>() };
|
||||
auto s2{ std::make_unique<ForwardCostMockup>() };
|
||||
|
||||
container.computeWithStages({ std::move(s1), std::move(s2) });
|
||||
EXPECT_EQ(container.solutions().front()->cost(), 2 * STAGE_COST) << "by default stage costs are added";
|
||||
}
|
||||
|
||||
{
|
||||
auto s1{ std::make_unique<ForwardMockup>() };
|
||||
auto s1{ std::make_unique<ForwardCostMockup>() };
|
||||
auto constant{ std::make_shared<cost::Constant>(1.0) };
|
||||
s1->setCostTerm(constant);
|
||||
auto s2{ std::make_unique<ForwardMockup>() };
|
||||
auto s2{ std::make_unique<ForwardCostMockup>() };
|
||||
|
||||
container.computeWithStages({ std::move(s1), std::move(s2) });
|
||||
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST + constant->cost)
|
||||
@ -334,8 +308,8 @@ TEST(CostTerm, CompositeSolutions) {
|
||||
}
|
||||
|
||||
{
|
||||
auto s1{ std::make_unique<ForwardMockup>() };
|
||||
auto s2{ std::make_unique<ForwardMockup>() };
|
||||
auto s1{ std::make_unique<ForwardCostMockup>() };
|
||||
auto s2{ std::make_unique<ForwardCostMockup>() };
|
||||
auto c1{ std::make_unique<SerialContainer>() };
|
||||
auto constant1{ std::make_shared<cost::Constant>(1.0) };
|
||||
s1->setCostTerm(constant1);
|
||||
@ -343,7 +317,7 @@ TEST(CostTerm, CompositeSolutions) {
|
||||
c1->add(std::move(s1));
|
||||
c1->add(std::move(s2));
|
||||
|
||||
auto s3{ std::make_unique<ForwardMockup>() };
|
||||
auto s3{ std::make_unique<ForwardCostMockup>() };
|
||||
auto constant2{ std::make_shared<cost::Constant>(9.0) };
|
||||
s3->setCostTerm(constant2);
|
||||
|
||||
@ -356,15 +330,15 @@ TEST(CostTerm, CompositeSolutions) {
|
||||
TEST(CostTerm, CompositeSolutionsContainerCost) {
|
||||
Standalone<SerialContainer> container{ getModel() };
|
||||
|
||||
auto s1{ std::make_unique<ForwardMockup>() };
|
||||
auto s1{ std::make_unique<ForwardTrajectoryMockup>() };
|
||||
auto s1_ptr{ s1.get() };
|
||||
auto s2{ std::make_unique<ForwardMockup>() };
|
||||
auto s2{ std::make_unique<ForwardTrajectoryMockup>() };
|
||||
|
||||
auto c1{ std::make_unique<SerialContainer>() };
|
||||
c1->add(std::move(s1));
|
||||
c1->add(std::move(s2));
|
||||
|
||||
auto s3{ std::make_unique<ForwardMockup>() };
|
||||
auto s3{ std::make_unique<ForwardTrajectoryMockup>() };
|
||||
|
||||
container.setCostTerm(std::make_unique<cost::TrajectoryDuration>());
|
||||
container.computeWithStages({ std::move(c1), std::move(s3) });
|
||||
|
||||
@ -5,6 +5,7 @@
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/utils/robot_model_test_utils.h>
|
||||
|
||||
#include "stage_mockups.h"
|
||||
#include "models.h"
|
||||
#include <list>
|
||||
#include <memory>
|
||||
@ -13,105 +14,11 @@
|
||||
using namespace moveit::task_constructor;
|
||||
using namespace planning_scene;
|
||||
|
||||
MOVEIT_STRUCT_FORWARD(PredefinedCosts);
|
||||
struct PredefinedCosts : CostTerm
|
||||
{
|
||||
mutable std::list<double> costs_; // list of costs to assign
|
||||
mutable double cost_ = 0.0; // last assigned cost
|
||||
bool finite_; // finite number of compute() attempts?
|
||||
|
||||
PredefinedCosts(bool finite, std::list<double>&& costs) : costs_(std::move(costs)), finite_(finite) {}
|
||||
bool exhausted() const { return finite_ && costs_.empty(); }
|
||||
double cost() const {
|
||||
if (!costs_.empty()) {
|
||||
cost_ = costs_.front();
|
||||
costs_.pop_front();
|
||||
}
|
||||
return cost_;
|
||||
}
|
||||
|
||||
double operator()(const SubTrajectory& /*s*/, std::string& /*comment*/) const override { return cost(); }
|
||||
double operator()(const SolutionSequence& /*s*/, std::string& /*comment*/) const override { return cost(); }
|
||||
double operator()(const WrappedSolution& /*s*/, std::string& /*comment*/) const override { return cost(); }
|
||||
};
|
||||
|
||||
/** Generator creating solutions with given costs */
|
||||
struct GeneratorMockup : Generator
|
||||
{
|
||||
PlanningScenePtr ps_;
|
||||
PredefinedCosts costs_;
|
||||
static unsigned int id_;
|
||||
|
||||
GeneratorMockup(std::initializer_list<double> costs = { 0.0 })
|
||||
: Generator("GEN" + std::to_string(++id_)), costs_(true, costs) {}
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
|
||||
ps_.reset(new PlanningScene(robot_model));
|
||||
Generator::init(robot_model);
|
||||
}
|
||||
|
||||
bool canCompute() const override { return !costs_.exhausted(); }
|
||||
void compute() override { spawn(InterfaceState(ps_), costs_.cost()); }
|
||||
};
|
||||
|
||||
struct PropagatorMockup : public PropagatingEitherWay
|
||||
{
|
||||
PredefinedCosts costs_;
|
||||
std::size_t solutions_per_compute_;
|
||||
|
||||
unsigned int calls_ = 0;
|
||||
|
||||
PropagatorMockup(std::initializer_list<double> costs = { 0.0 }, std::size_t solutions_per_compute = 1)
|
||||
: PropagatingEitherWay(), costs_(false, costs), solutions_per_compute_(solutions_per_compute) {}
|
||||
|
||||
void computeForward(const InterfaceState& from) override {
|
||||
++calls_;
|
||||
for (std::size_t i = 0; i < solutions_per_compute_; ++i) {
|
||||
SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost());
|
||||
sendForward(from, InterfaceState(from.scene()->diff()), std::move(solution));
|
||||
}
|
||||
}
|
||||
void computeBackward(const InterfaceState& to) override {
|
||||
++calls_;
|
||||
for (std::size_t i = 0; i < solutions_per_compute_; ++i) {
|
||||
SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost());
|
||||
sendBackward(InterfaceState(to.scene()->diff()), to, std::move(solution));
|
||||
}
|
||||
}
|
||||
};
|
||||
struct ForwardMockup : public PropagatorMockup
|
||||
{
|
||||
static unsigned int id_;
|
||||
|
||||
ForwardMockup(std::initializer_list<double> costs = { 0.0 }, std::size_t solutions_per_compute = 1)
|
||||
: PropagatorMockup(costs, solutions_per_compute) {
|
||||
restrictDirection(FORWARD);
|
||||
setName("FW" + std::to_string(++id_));
|
||||
}
|
||||
};
|
||||
struct BackwardMockup : public PropagatorMockup
|
||||
{
|
||||
static unsigned int id_;
|
||||
|
||||
BackwardMockup(std::initializer_list<double> costs = { 0.0 }) : PropagatorMockup(costs) {
|
||||
restrictDirection(BACKWARD);
|
||||
setName("BW" + std::to_string(++id_));
|
||||
}
|
||||
};
|
||||
|
||||
/* Forward propagator, contributing no solutions at all */
|
||||
struct ForwardDummy : PropagatingForward
|
||||
{
|
||||
using PropagatingForward::PropagatingForward;
|
||||
void computeForward(const InterfaceState& /*from*/) override {}
|
||||
};
|
||||
|
||||
/* Connect creating solutions with given costs */
|
||||
struct Connect : stages::Connect
|
||||
{
|
||||
PlanningScenePtr ps_;
|
||||
PredefinedCostsPtr costs_;
|
||||
unsigned int calls_ = 0;
|
||||
unsigned int calls_{ 0 };
|
||||
static unsigned int id_;
|
||||
|
||||
static GroupPlannerVector getPlanners() {
|
||||
@ -119,9 +26,9 @@ struct Connect : stages::Connect
|
||||
return { { "group", planner }, { "eef_group", planner } };
|
||||
}
|
||||
|
||||
Connect(std::initializer_list<double> costs = {}, bool enforce_sequential = false)
|
||||
Connect(std::initializer_list<double> costs = { 0.0 }, bool enforce_sequential = false)
|
||||
: stages::Connect("CON" + std::to_string(++id_), getPlanners()) {
|
||||
costs_ = std::make_shared<PredefinedCosts>(false, costs);
|
||||
costs_ = std::make_shared<PredefinedCosts>(costs, false);
|
||||
setCostTerm(costs_);
|
||||
if (enforce_sequential)
|
||||
setProperty("merge_mode", SEQUENTIAL);
|
||||
@ -133,25 +40,17 @@ struct Connect : stages::Connect
|
||||
};
|
||||
|
||||
constexpr double INF = std::numeric_limits<double>::infinity();
|
||||
unsigned int GeneratorMockup::id_ = 0;
|
||||
unsigned int ForwardMockup::id_ = 0;
|
||||
unsigned int BackwardMockup::id_ = 0;
|
||||
unsigned int Connect::id_ = 0;
|
||||
|
||||
struct TestBase : public testing::Test
|
||||
{
|
||||
Task task;
|
||||
TestBase() {
|
||||
resetIds();
|
||||
resetMockupIds();
|
||||
Connect::id_ = 0;
|
||||
task.setRobotModel(getModel());
|
||||
}
|
||||
|
||||
void resetIds() {
|
||||
GeneratorMockup::id_ = 0;
|
||||
ForwardMockup::id_ = 0;
|
||||
BackwardMockup::id_ = 0;
|
||||
Connect::id_ = 0;
|
||||
}
|
||||
template <typename C, typename S>
|
||||
auto add(C& container, S* stage) -> S* {
|
||||
container.add(Stage::pointer(stage));
|
||||
@ -162,11 +61,11 @@ struct TestBase : public testing::Test
|
||||
using ConnectConnect = TestBase;
|
||||
// https://github.com/ros-planning/moveit_task_constructor/issues/182
|
||||
TEST_F(ConnectConnect, SuccSucc) {
|
||||
add(task, new GeneratorMockup({ 1, 2, 3 }));
|
||||
add(task, new GeneratorMockup({ 1.0, 2.0, 3.0 }));
|
||||
add(task, new Connect());
|
||||
add(task, new GeneratorMockup({ 10, 20 }));
|
||||
add(task, new GeneratorMockup({ 10.0, 20.0 }));
|
||||
add(task, new Connect());
|
||||
add(task, new GeneratorMockup());
|
||||
add(task, new GeneratorMockup({ 0.0 }));
|
||||
|
||||
EXPECT_TRUE(task.plan());
|
||||
ASSERT_EQ(task.solutions().size(), 3u * 2u);
|
||||
@ -185,7 +84,7 @@ TEST_F(ConnectConnect, FailSucc) {
|
||||
add(task, new GeneratorMockup());
|
||||
add(task, new Connect());
|
||||
add(task, new GeneratorMockup());
|
||||
add(task, new ForwardDummy());
|
||||
add(task, new ForwardMockup(PredefinedCosts::constant(0.0), 0));
|
||||
|
||||
EXPECT_FALSE(task.plan());
|
||||
}
|
||||
@ -199,7 +98,7 @@ TEST_F(Pruning, PropagatorFailure) {
|
||||
EXPECT_FALSE(task.plan());
|
||||
ASSERT_EQ(task.solutions().size(), 0u);
|
||||
// ForwardMockup fails, so the backward stage should never compute
|
||||
EXPECT_EQ(back->calls_, 0u);
|
||||
EXPECT_EQ(back->runs_, 0u);
|
||||
}
|
||||
|
||||
TEST_F(Pruning, PruningMultiForward) {
|
||||
@ -207,7 +106,7 @@ TEST_F(Pruning, PruningMultiForward) {
|
||||
add(task, new BackwardMockup());
|
||||
add(task, new GeneratorMockup());
|
||||
// spawn two solutions for the only incoming state
|
||||
add(task, new ForwardMockup({ 0, 0 }, 2));
|
||||
add(task, new ForwardMockup(PredefinedCosts{ { 0.0, 0.0 } }, 2));
|
||||
// fail to extend the second solution
|
||||
add(task, new ForwardMockup({ 0, INF }));
|
||||
|
||||
@ -288,7 +187,7 @@ TEST_F(Pruning, PropagateFromContainerPull) {
|
||||
EXPECT_FALSE(task.plan());
|
||||
|
||||
// the failure inside the container should prune computing of back
|
||||
EXPECT_EQ(back->calls_, 0u);
|
||||
EXPECT_EQ(back->runs_, 0u);
|
||||
}
|
||||
|
||||
TEST_F(Pruning, PropagateFromContainerPush) {
|
||||
@ -318,5 +217,5 @@ TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) {
|
||||
EXPECT_TRUE(task.plan());
|
||||
|
||||
// the failure in one branch of Alternatives must not prune computing back
|
||||
EXPECT_EQ(back->calls_, 1u);
|
||||
EXPECT_EQ(back->runs_, 1u);
|
||||
}
|
||||
|
||||
@ -6,52 +6,36 @@
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
|
||||
#include "stage_mockups.h"
|
||||
#include <ros/console.h>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
using namespace planning_scene;
|
||||
|
||||
class GeneratorMockup : public Generator
|
||||
struct StandaloneGeneratorMockup : public GeneratorMockup
|
||||
{
|
||||
PlanningScenePtr ps;
|
||||
InterfacePtr prev;
|
||||
InterfacePtr next;
|
||||
|
||||
public:
|
||||
GeneratorMockup() : Generator("generator") {
|
||||
StandaloneGeneratorMockup(std::initializer_list<double>&& costs)
|
||||
: StandaloneGeneratorMockup{ PredefinedCosts{ std::move(costs), true } } {}
|
||||
|
||||
StandaloneGeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ { 0.0 }, true })
|
||||
: GeneratorMockup{ std::move(costs) } {
|
||||
prev.reset(new Interface);
|
||||
next.reset(new Interface);
|
||||
pimpl()->setPrevEnds(prev);
|
||||
pimpl()->setNextStarts(next);
|
||||
}
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
|
||||
ps.reset((new PlanningScene(robot_model)));
|
||||
Generator::init(robot_model);
|
||||
}
|
||||
|
||||
bool canCompute() const override { return true; }
|
||||
void compute() override {
|
||||
InterfaceState state(ps);
|
||||
state.properties().set("target_pose", geometry_msgs::PoseStamped());
|
||||
spawn(std::move(state), 0.0);
|
||||
}
|
||||
};
|
||||
|
||||
class ConnectMockup : public Connecting
|
||||
{
|
||||
public:
|
||||
using Connecting::compatible;
|
||||
void compute(const InterfaceState& from, const InterfaceState& to) override {}
|
||||
};
|
||||
|
||||
TEST(Stage, registerCallbacks) {
|
||||
GeneratorMockup g;
|
||||
StandaloneGeneratorMockup g{ PredefinedCosts::constant(0.0) };
|
||||
g.init(getModel());
|
||||
|
||||
uint called = 0;
|
||||
auto cb = [&called](const SolutionBase& s) {
|
||||
auto cb = [&called](const SolutionBase& /* s */) {
|
||||
++called;
|
||||
return true;
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user