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)
|
if (CATKIN_ENABLE_TESTING)
|
||||||
find_package(rostest REQUIRED)
|
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})
|
target_link_libraries(gtest_utils ${PROJECT_NAME})
|
||||||
|
|
||||||
catkin_add_gtest(${PROJECT_NAME}-test-stage test_stage.cpp)
|
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/task_constructor/stages/fixed_state.h>
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
|
|
||||||
|
#include "stage_mockups.h"
|
||||||
#include "models.h"
|
#include "models.h"
|
||||||
#include "gtest_value_printers.h"
|
#include "gtest_value_printers.h"
|
||||||
|
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include <initializer_list>
|
#include <initializer_list>
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
@ -13,101 +15,6 @@
|
|||||||
|
|
||||||
using namespace moveit::task_constructor;
|
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
|
enum StageType
|
||||||
{
|
{
|
||||||
GEN,
|
GEN,
|
||||||
@ -116,23 +23,23 @@ enum StageType
|
|||||||
ANY,
|
ANY,
|
||||||
CONN
|
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) {
|
for (StageType t : types) {
|
||||||
switch (t) {
|
switch (t) {
|
||||||
case GEN:
|
case GEN:
|
||||||
c.add(std::make_unique<GeneratorMockup>(runs));
|
c.add(std::make_unique<GeneratorMockup>());
|
||||||
break;
|
break;
|
||||||
case FW:
|
case FW:
|
||||||
c.add(std::make_unique<ForwardMockup>(runs));
|
c.add(std::make_unique<ForwardMockup>());
|
||||||
break;
|
break;
|
||||||
case BW:
|
case BW:
|
||||||
c.add(std::make_unique<BackwardMockup>(runs));
|
c.add(std::make_unique<BackwardMockup>());
|
||||||
break;
|
break;
|
||||||
case ANY:
|
case ANY:
|
||||||
c.add(std::make_unique<PropagatorMockup>(runs, runs));
|
c.add(std::make_unique<PropagatorMockup>());
|
||||||
break;
|
break;
|
||||||
case CONN:
|
case CONN:
|
||||||
c.add(std::make_unique<ConnectMockup>(runs));
|
c.add(std::make_unique<ConnectMockup>());
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -215,7 +122,7 @@ protected:
|
|||||||
}
|
}
|
||||||
void initContainer(const std::initializer_list<StageType>& types = {}) {
|
void initContainer(const std::initializer_list<StageType>& types = {}) {
|
||||||
container.clear();
|
container.clear();
|
||||||
MOCK_ID = 0;
|
resetMockupIds();
|
||||||
append(container, types);
|
append(container, types);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -505,7 +412,7 @@ TEST_F(SerialTest, interface_detection) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
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);
|
auto container = new T(name);
|
||||||
append(*container, children);
|
append(*container, children);
|
||||||
return Stage::pointer(container);
|
return Stage::pointer(container);
|
||||||
@ -669,13 +576,13 @@ TEST_F(ParallelTest, init_any) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST(Task, move) {
|
TEST(Task, move) {
|
||||||
MOCK_ID = 0;
|
resetMockupIds();
|
||||||
Task t1("foo");
|
Task t1("foo");
|
||||||
t1.add(std::make_unique<GeneratorMockup>());
|
t1.add(std::make_unique<GeneratorMockup>());
|
||||||
t1.add(std::make_unique<GeneratorMockup>());
|
t1.add(std::make_unique<GeneratorMockup>());
|
||||||
EXPECT_EQ(t1.stages()->numChildren(), 2u);
|
EXPECT_EQ(t1.stages()->numChildren(), 2u);
|
||||||
|
|
||||||
MOCK_ID = 0;
|
resetMockupIds();
|
||||||
Task t2 = std::move(t1);
|
Task t2 = std::move(t1);
|
||||||
EXPECT_EQ(t2.stages()->numChildren(), 2u);
|
EXPECT_EQ(t2.stages()->numChildren(), 2u);
|
||||||
EXPECT_EQ(t1.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move)
|
EXPECT_EQ(t1.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move)
|
||||||
@ -692,13 +599,13 @@ TEST(Task, reuse) {
|
|||||||
t.setRobotModel(robot_model);
|
t.setRobotModel(robot_model);
|
||||||
|
|
||||||
auto configure = [](Task& t) {
|
auto configure = [](Task& t) {
|
||||||
MOCK_ID = 0;
|
resetMockupIds();
|
||||||
auto ref = new stages::FixedState("fixed");
|
auto ref = new stages::FixedState("fixed");
|
||||||
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
|
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
|
||||||
ref->setState(scene);
|
ref->setState(scene);
|
||||||
|
|
||||||
t.add(Stage::pointer(ref));
|
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));
|
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) {
|
TEST(Task, timeout) {
|
||||||
MOCK_ID = 0;
|
resetMockupIds();
|
||||||
Task t;
|
Task t;
|
||||||
t.setRobotModel(getModel());
|
t.setRobotModel(getModel());
|
||||||
|
|
||||||
auto timeout = std::chrono::milliseconds(10);
|
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));
|
t.add(std::make_unique<TimedForwardMockup>(timeout));
|
||||||
|
|
||||||
// no timeout, but limited number of solutions
|
// no timeout, but limited number of solutions
|
||||||
@ -751,15 +671,15 @@ TEST(Task, timeout) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST(Fallback, failing) {
|
TEST(Fallback, failing) {
|
||||||
MOCK_ID = 0;
|
resetMockupIds();
|
||||||
Task t;
|
Task t;
|
||||||
t.setRobotModel(getModel());
|
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");
|
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
|
||||||
fallback->add(std::make_unique<ForwardMockup>());
|
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0), 0));
|
||||||
fallback->add(std::make_unique<ForwardMockup>());
|
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0), 0));
|
||||||
t.add(std::move(fallback));
|
t.add(std::move(fallback));
|
||||||
|
|
||||||
EXPECT_FALSE(t.plan());
|
EXPECT_FALSE(t.plan());
|
||||||
|
|||||||
@ -11,67 +11,49 @@
|
|||||||
|
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
|
#include "stage_mockups.h"
|
||||||
|
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
using namespace planning_scene;
|
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 };
|
const double TRAJECTORY_DURATION{ 9.0 };
|
||||||
|
|
||||||
class GeneratorMockup : public Generator
|
struct GeneratorCostMockup : public GeneratorMockup
|
||||||
{
|
{
|
||||||
PlanningScenePtr ps;
|
GeneratorCostMockup() : GeneratorMockup{ PredefinedCosts{ { STAGE_COST }, true } } {}
|
||||||
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);
|
|
||||||
}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
class ConnectMockup : public Connecting
|
struct ConnectCostMockup : public ConnectMockup
|
||||||
{
|
{
|
||||||
using Connecting::Connecting;
|
ConnectCostMockup() : ConnectMockup{ PredefinedCosts::constant(STAGE_COST) } {}
|
||||||
|
|
||||||
void compute(const InterfaceState& from, const InterfaceState& to) override {
|
|
||||||
auto solution{ std::make_shared<SubTrajectory>() };
|
|
||||||
solution->setCost(STAGE_COST);
|
|
||||||
connect(from, to, solution);
|
|
||||||
}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
class ForwardMockup : public PropagatingForward
|
struct ForwardCostMockup : public ForwardMockup
|
||||||
{
|
{
|
||||||
PlanningScenePtr ps;
|
ForwardCostMockup() : ForwardMockup{ PredefinedCosts::constant(STAGE_COST) } {}
|
||||||
|
};
|
||||||
|
|
||||||
public:
|
struct BackwardCostMockup : public BackwardMockup
|
||||||
using PropagatingForward::PropagatingForward;
|
{
|
||||||
|
BackwardCostMockup() : BackwardMockup{ PredefinedCosts::constant(STAGE_COST) } {}
|
||||||
|
};
|
||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
|
struct ForwardTrajectoryMockup : public ForwardMockup
|
||||||
ps.reset(new PlanningScene(robot_model));
|
{
|
||||||
PropagatingForward::init(robot_model);
|
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 {
|
void computeForward(const InterfaceState& from) override {
|
||||||
|
++runs_;
|
||||||
|
|
||||||
|
for (size_t i = 0; i < solutions_per_compute_; ++i) {
|
||||||
SubTrajectory solution;
|
SubTrajectory solution;
|
||||||
auto traj{ std::make_shared<robot_trajectory::RobotTrajectory>(ps->getRobotModel(),
|
auto traj{ std::make_shared<robot_trajectory::RobotTrajectory>(from.scene()->getRobotModel(), nullptr) };
|
||||||
ps->getRobotModel()->getJointModelGroups()[0]) };
|
planning_scene::PlanningScenePtr ps{ from.scene()->diff() };
|
||||||
traj->addSuffixWayPoint(ps->getCurrentState(), 0.0);
|
traj->addSuffixWayPoint(ps->getCurrentState(), 0.0);
|
||||||
traj->addSuffixWayPoint(ps->getCurrentState(), TRAJECTORY_DURATION);
|
traj->addSuffixWayPoint(ps->getCurrentState(), TRAJECTORY_DURATION);
|
||||||
solution.setTrajectory(traj);
|
solution.setTrajectory(traj);
|
||||||
@ -79,20 +61,8 @@ public:
|
|||||||
InterfaceState to(from);
|
InterfaceState to(from);
|
||||||
|
|
||||||
sendForward(from, std::move(to), std::move(solution));
|
sendForward(from, std::move(to), std::move(solution));
|
||||||
};
|
}
|
||||||
};
|
}
|
||||||
|
|
||||||
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));
|
|
||||||
};
|
|
||||||
};
|
};
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
@ -105,7 +75,9 @@ public:
|
|||||||
InterfaceStatePtr state_start, state_end;
|
InterfaceStatePtr state_start, state_end;
|
||||||
|
|
||||||
Standalone(const moveit::core::RobotModelConstPtr& robot)
|
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
|
// reset and prepare for a compute step
|
||||||
void prepare() {
|
void prepare() {
|
||||||
@ -173,7 +145,7 @@ TEST(CostTerm, SolutionConnected) {
|
|||||||
const moveit::core::RobotModelConstPtr robot{ getModel() };
|
const moveit::core::RobotModelConstPtr robot{ getModel() };
|
||||||
|
|
||||||
Standalone<SerialContainer> container(robot);
|
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
|
// custom CostTerm to verify SubTrajectory is hooked up to its states & creator
|
||||||
class VerifySolutionCostTerm : public TrajectoryCostTerm
|
class VerifySolutionCostTerm : public TrajectoryCostTerm
|
||||||
@ -193,38 +165,40 @@ TEST(CostTerm, SolutionConnected) {
|
|||||||
const_cast<const SerialContainerPrivate*>(container_.pimpl())->internalToExternalMap().at(s.end()))
|
const_cast<const SerialContainerPrivate*>(container_.pimpl())->internalToExternalMap().at(s.end()))
|
||||||
<< "SubTrajectory is not connected to its expected end InterfaceState";
|
<< "SubTrajectory is not connected to its expected end InterfaceState";
|
||||||
EXPECT_EQ(s.creator(), creator_);
|
EXPECT_EQ(s.creator(), creator_);
|
||||||
return 1.0;
|
return TERM_COST;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
stage->setCostTerm(std::make_unique<VerifySolutionCostTerm>(container, &*stage));
|
stage->setCostTerm(std::make_unique<VerifySolutionCostTerm>(container, &*stage));
|
||||||
container.computeWithStages({ std::move(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) {
|
TEST(CostTerm, SetLambdaCostTerm) {
|
||||||
const moveit::core::RobotModelConstPtr robot{ getModel() };
|
const moveit::core::RobotModelConstPtr robot{ getModel() };
|
||||||
|
|
||||||
Standalone<SerialContainer> container(robot);
|
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; });
|
stage->setCostTerm([](auto&& /*s*/, auto&& /*comment*/) { return 1.0; });
|
||||||
container.computeWithStages({ std::move(stage) });
|
container.computeWithStages({ std::move(stage) });
|
||||||
EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "can use full lambda signature";
|
EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "can use full lambda signature";
|
||||||
|
|
||||||
stage = std::make_unique<GeneratorMockup>();
|
const std::string cost_term_comment{ "I want the user to see this" };
|
||||||
stage->setCostTerm([](auto&& /*s*/, auto&& comment) {
|
stage = std::make_unique<GeneratorCostMockup>();
|
||||||
comment = "I want the user to see this";
|
stage->setCostTerm([&](auto&& /*s*/, auto&& comment) {
|
||||||
|
comment = cost_term_comment;
|
||||||
return 1.0;
|
return 1.0;
|
||||||
});
|
});
|
||||||
container.computeWithStages({ std::move(stage) });
|
container.computeWithStages({ std::move(stage) });
|
||||||
auto sol = std::dynamic_pointer_cast<const SolutionSequence>(container.solutions().front());
|
auto sol = std::dynamic_pointer_cast<const SolutionSequence>(container.solutions().front());
|
||||||
EXPECT_EQ(sol->cost(), 1.0);
|
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) {
|
TEST(CostTerm, CostOverwrite) {
|
||||||
@ -232,14 +206,14 @@ TEST(CostTerm, CostOverwrite) {
|
|||||||
|
|
||||||
Standalone<SerialContainer> container(robot);
|
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";
|
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";
|
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost";
|
||||||
|
|
||||||
auto constant_cost{ std::make_shared<cost::Constant>(1.0) };
|
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";
|
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) };
|
auto constant{ std::make_shared<cost::Constant>(1.0) };
|
||||||
|
|
||||||
// already tested above
|
// 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);
|
// 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";
|
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";
|
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";
|
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() };
|
moveit::core::RobotModelPtr robot{ getModel() };
|
||||||
Standalone<stages::PassThrough> container(robot);
|
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) };
|
auto constant{ std::make_shared<cost::Constant>(84.0) };
|
||||||
stage->setCostTerm(constant);
|
stage->setCostTerm(constant);
|
||||||
|
|
||||||
@ -283,7 +257,7 @@ TEST(CostTerm, PassThroughOverwritesCost) {
|
|||||||
moveit::core::RobotModelPtr robot{ getModel() };
|
moveit::core::RobotModelPtr robot{ getModel() };
|
||||||
Standalone<stages::PassThrough> container(robot);
|
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) };
|
auto constant_inner{ std::make_shared<cost::Constant>(84.0) };
|
||||||
stage->setCostTerm(constant_inner);
|
stage->setCostTerm(constant_inner);
|
||||||
|
|
||||||
@ -300,7 +274,7 @@ TEST(CostTerm, PassThroughCanModifyCost) {
|
|||||||
moveit::core::RobotModelPtr robot{ getModel() };
|
moveit::core::RobotModelPtr robot{ getModel() };
|
||||||
Standalone<stages::PassThrough> container(robot);
|
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) };
|
auto constant{ std::make_shared<cost::Constant>(8.0) };
|
||||||
stage->setCostTerm(constant);
|
stage->setCostTerm(constant);
|
||||||
container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); });
|
container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); });
|
||||||
@ -315,18 +289,18 @@ TEST(CostTerm, CompositeSolutions) {
|
|||||||
Standalone<SerialContainer> container{ getModel() };
|
Standalone<SerialContainer> container{ getModel() };
|
||||||
|
|
||||||
{
|
{
|
||||||
auto s1{ std::make_unique<ForwardMockup>() };
|
auto s1{ std::make_unique<ForwardCostMockup>() };
|
||||||
auto s2{ std::make_unique<ForwardMockup>() };
|
auto s2{ std::make_unique<ForwardCostMockup>() };
|
||||||
|
|
||||||
container.computeWithStages({ std::move(s1), std::move(s2) });
|
container.computeWithStages({ std::move(s1), std::move(s2) });
|
||||||
EXPECT_EQ(container.solutions().front()->cost(), 2 * STAGE_COST) << "by default stage costs are added";
|
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) };
|
auto constant{ std::make_shared<cost::Constant>(1.0) };
|
||||||
s1->setCostTerm(constant);
|
s1->setCostTerm(constant);
|
||||||
auto s2{ std::make_unique<ForwardMockup>() };
|
auto s2{ std::make_unique<ForwardCostMockup>() };
|
||||||
|
|
||||||
container.computeWithStages({ std::move(s1), std::move(s2) });
|
container.computeWithStages({ std::move(s1), std::move(s2) });
|
||||||
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST + constant->cost)
|
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST + constant->cost)
|
||||||
@ -334,8 +308,8 @@ TEST(CostTerm, CompositeSolutions) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
auto s1{ std::make_unique<ForwardMockup>() };
|
auto s1{ std::make_unique<ForwardCostMockup>() };
|
||||||
auto s2{ std::make_unique<ForwardMockup>() };
|
auto s2{ std::make_unique<ForwardCostMockup>() };
|
||||||
auto c1{ std::make_unique<SerialContainer>() };
|
auto c1{ std::make_unique<SerialContainer>() };
|
||||||
auto constant1{ std::make_shared<cost::Constant>(1.0) };
|
auto constant1{ std::make_shared<cost::Constant>(1.0) };
|
||||||
s1->setCostTerm(constant1);
|
s1->setCostTerm(constant1);
|
||||||
@ -343,7 +317,7 @@ TEST(CostTerm, CompositeSolutions) {
|
|||||||
c1->add(std::move(s1));
|
c1->add(std::move(s1));
|
||||||
c1->add(std::move(s2));
|
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) };
|
auto constant2{ std::make_shared<cost::Constant>(9.0) };
|
||||||
s3->setCostTerm(constant2);
|
s3->setCostTerm(constant2);
|
||||||
|
|
||||||
@ -356,15 +330,15 @@ TEST(CostTerm, CompositeSolutions) {
|
|||||||
TEST(CostTerm, CompositeSolutionsContainerCost) {
|
TEST(CostTerm, CompositeSolutionsContainerCost) {
|
||||||
Standalone<SerialContainer> container{ getModel() };
|
Standalone<SerialContainer> container{ getModel() };
|
||||||
|
|
||||||
auto s1{ std::make_unique<ForwardMockup>() };
|
auto s1{ std::make_unique<ForwardTrajectoryMockup>() };
|
||||||
auto s1_ptr{ s1.get() };
|
auto s1_ptr{ s1.get() };
|
||||||
auto s2{ std::make_unique<ForwardMockup>() };
|
auto s2{ std::make_unique<ForwardTrajectoryMockup>() };
|
||||||
|
|
||||||
auto c1{ std::make_unique<SerialContainer>() };
|
auto c1{ std::make_unique<SerialContainer>() };
|
||||||
c1->add(std::move(s1));
|
c1->add(std::move(s1));
|
||||||
c1->add(std::move(s2));
|
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.setCostTerm(std::make_unique<cost::TrajectoryDuration>());
|
||||||
container.computeWithStages({ std::move(c1), std::move(s3) });
|
container.computeWithStages({ std::move(c1), std::move(s3) });
|
||||||
|
|||||||
@ -5,6 +5,7 @@
|
|||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <moveit/utils/robot_model_test_utils.h>
|
#include <moveit/utils/robot_model_test_utils.h>
|
||||||
|
|
||||||
|
#include "stage_mockups.h"
|
||||||
#include "models.h"
|
#include "models.h"
|
||||||
#include <list>
|
#include <list>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
@ -13,105 +14,11 @@
|
|||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
using namespace planning_scene;
|
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 */
|
/* Connect creating solutions with given costs */
|
||||||
struct Connect : stages::Connect
|
struct Connect : stages::Connect
|
||||||
{
|
{
|
||||||
PlanningScenePtr ps_;
|
|
||||||
PredefinedCostsPtr costs_;
|
PredefinedCostsPtr costs_;
|
||||||
unsigned int calls_ = 0;
|
unsigned int calls_{ 0 };
|
||||||
static unsigned int id_;
|
static unsigned int id_;
|
||||||
|
|
||||||
static GroupPlannerVector getPlanners() {
|
static GroupPlannerVector getPlanners() {
|
||||||
@ -119,9 +26,9 @@ struct Connect : stages::Connect
|
|||||||
return { { "group", planner }, { "eef_group", planner } };
|
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()) {
|
: stages::Connect("CON" + std::to_string(++id_), getPlanners()) {
|
||||||
costs_ = std::make_shared<PredefinedCosts>(false, costs);
|
costs_ = std::make_shared<PredefinedCosts>(costs, false);
|
||||||
setCostTerm(costs_);
|
setCostTerm(costs_);
|
||||||
if (enforce_sequential)
|
if (enforce_sequential)
|
||||||
setProperty("merge_mode", SEQUENTIAL);
|
setProperty("merge_mode", SEQUENTIAL);
|
||||||
@ -133,25 +40,17 @@ struct Connect : stages::Connect
|
|||||||
};
|
};
|
||||||
|
|
||||||
constexpr double INF = std::numeric_limits<double>::infinity();
|
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;
|
unsigned int Connect::id_ = 0;
|
||||||
|
|
||||||
struct TestBase : public testing::Test
|
struct TestBase : public testing::Test
|
||||||
{
|
{
|
||||||
Task task;
|
Task task;
|
||||||
TestBase() {
|
TestBase() {
|
||||||
resetIds();
|
resetMockupIds();
|
||||||
|
Connect::id_ = 0;
|
||||||
task.setRobotModel(getModel());
|
task.setRobotModel(getModel());
|
||||||
}
|
}
|
||||||
|
|
||||||
void resetIds() {
|
|
||||||
GeneratorMockup::id_ = 0;
|
|
||||||
ForwardMockup::id_ = 0;
|
|
||||||
BackwardMockup::id_ = 0;
|
|
||||||
Connect::id_ = 0;
|
|
||||||
}
|
|
||||||
template <typename C, typename S>
|
template <typename C, typename S>
|
||||||
auto add(C& container, S* stage) -> S* {
|
auto add(C& container, S* stage) -> S* {
|
||||||
container.add(Stage::pointer(stage));
|
container.add(Stage::pointer(stage));
|
||||||
@ -162,11 +61,11 @@ struct TestBase : public testing::Test
|
|||||||
using ConnectConnect = TestBase;
|
using ConnectConnect = TestBase;
|
||||||
// https://github.com/ros-planning/moveit_task_constructor/issues/182
|
// https://github.com/ros-planning/moveit_task_constructor/issues/182
|
||||||
TEST_F(ConnectConnect, SuccSucc) {
|
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 Connect());
|
||||||
add(task, new GeneratorMockup({ 10, 20 }));
|
add(task, new GeneratorMockup({ 10.0, 20.0 }));
|
||||||
add(task, new Connect());
|
add(task, new Connect());
|
||||||
add(task, new GeneratorMockup());
|
add(task, new GeneratorMockup({ 0.0 }));
|
||||||
|
|
||||||
EXPECT_TRUE(task.plan());
|
EXPECT_TRUE(task.plan());
|
||||||
ASSERT_EQ(task.solutions().size(), 3u * 2u);
|
ASSERT_EQ(task.solutions().size(), 3u * 2u);
|
||||||
@ -185,7 +84,7 @@ TEST_F(ConnectConnect, FailSucc) {
|
|||||||
add(task, new GeneratorMockup());
|
add(task, new GeneratorMockup());
|
||||||
add(task, new Connect());
|
add(task, new Connect());
|
||||||
add(task, new GeneratorMockup());
|
add(task, new GeneratorMockup());
|
||||||
add(task, new ForwardDummy());
|
add(task, new ForwardMockup(PredefinedCosts::constant(0.0), 0));
|
||||||
|
|
||||||
EXPECT_FALSE(task.plan());
|
EXPECT_FALSE(task.plan());
|
||||||
}
|
}
|
||||||
@ -199,7 +98,7 @@ TEST_F(Pruning, PropagatorFailure) {
|
|||||||
EXPECT_FALSE(task.plan());
|
EXPECT_FALSE(task.plan());
|
||||||
ASSERT_EQ(task.solutions().size(), 0u);
|
ASSERT_EQ(task.solutions().size(), 0u);
|
||||||
// ForwardMockup fails, so the backward stage should never compute
|
// ForwardMockup fails, so the backward stage should never compute
|
||||||
EXPECT_EQ(back->calls_, 0u);
|
EXPECT_EQ(back->runs_, 0u);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(Pruning, PruningMultiForward) {
|
TEST_F(Pruning, PruningMultiForward) {
|
||||||
@ -207,7 +106,7 @@ TEST_F(Pruning, PruningMultiForward) {
|
|||||||
add(task, new BackwardMockup());
|
add(task, new BackwardMockup());
|
||||||
add(task, new GeneratorMockup());
|
add(task, new GeneratorMockup());
|
||||||
// spawn two solutions for the only incoming state
|
// 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
|
// fail to extend the second solution
|
||||||
add(task, new ForwardMockup({ 0, INF }));
|
add(task, new ForwardMockup({ 0, INF }));
|
||||||
|
|
||||||
@ -288,7 +187,7 @@ TEST_F(Pruning, PropagateFromContainerPull) {
|
|||||||
EXPECT_FALSE(task.plan());
|
EXPECT_FALSE(task.plan());
|
||||||
|
|
||||||
// the failure inside the container should prune computing of back
|
// the failure inside the container should prune computing of back
|
||||||
EXPECT_EQ(back->calls_, 0u);
|
EXPECT_EQ(back->runs_, 0u);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(Pruning, PropagateFromContainerPush) {
|
TEST_F(Pruning, PropagateFromContainerPush) {
|
||||||
@ -318,5 +217,5 @@ TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) {
|
|||||||
EXPECT_TRUE(task.plan());
|
EXPECT_TRUE(task.plan());
|
||||||
|
|
||||||
// the failure in one branch of Alternatives must not prune computing back
|
// 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 <moveit/planning_scene/planning_scene.h>
|
||||||
#include <geometry_msgs/PoseStamped.h>
|
#include <geometry_msgs/PoseStamped.h>
|
||||||
|
|
||||||
|
#include "stage_mockups.h"
|
||||||
#include <ros/console.h>
|
#include <ros/console.h>
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
using namespace planning_scene;
|
using namespace planning_scene;
|
||||||
|
|
||||||
class GeneratorMockup : public Generator
|
struct StandaloneGeneratorMockup : public GeneratorMockup
|
||||||
{
|
{
|
||||||
PlanningScenePtr ps;
|
|
||||||
InterfacePtr prev;
|
InterfacePtr prev;
|
||||||
InterfacePtr next;
|
InterfacePtr next;
|
||||||
|
|
||||||
public:
|
StandaloneGeneratorMockup(std::initializer_list<double>&& costs)
|
||||||
GeneratorMockup() : Generator("generator") {
|
: StandaloneGeneratorMockup{ PredefinedCosts{ std::move(costs), true } } {}
|
||||||
|
|
||||||
|
StandaloneGeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ { 0.0 }, true })
|
||||||
|
: GeneratorMockup{ std::move(costs) } {
|
||||||
prev.reset(new Interface);
|
prev.reset(new Interface);
|
||||||
next.reset(new Interface);
|
next.reset(new Interface);
|
||||||
pimpl()->setPrevEnds(prev);
|
pimpl()->setPrevEnds(prev);
|
||||||
pimpl()->setNextStarts(next);
|
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) {
|
TEST(Stage, registerCallbacks) {
|
||||||
GeneratorMockup g;
|
StandaloneGeneratorMockup g{ PredefinedCosts::constant(0.0) };
|
||||||
g.init(getModel());
|
g.init(getModel());
|
||||||
|
|
||||||
uint called = 0;
|
uint called = 0;
|
||||||
auto cb = [&called](const SolutionBase& s) {
|
auto cb = [&called](const SolutionBase& /* s */) {
|
||||||
++called;
|
++called;
|
||||||
return true;
|
return true;
|
||||||
};
|
};
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user