Merge different mockup implementations

Co-authored-by: Jascha Kühn <57101356+j-kuehn@users.noreply.github.com>
This commit is contained in:
v4hn 2021-07-04 23:30:49 +02:00 committed by Michael Görner
parent 4b1f240c21
commit 74d33c4ec0
7 changed files with 369 additions and 350 deletions

View File

@ -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
View 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
View 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

View File

@ -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());

View File

@ -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) });

View File

@ -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);
}

View File

@ -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;
};