mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
add failing pruning test for branching propagator
This commit is contained in:
parent
a82b88355e
commit
f8f885966e
@ -57,17 +57,23 @@ struct GeneratorMockup : Generator
|
|||||||
class PropagatorMockup : public PropagatingEitherWay
|
class PropagatorMockup : public PropagatingEitherWay
|
||||||
{
|
{
|
||||||
PredefinedCosts costs_;
|
PredefinedCosts costs_;
|
||||||
|
std::size_t solutions_per_compute_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
PropagatorMockup(std::initializer_list<double> costs = { 0.0 }) : PropagatingEitherWay(), costs_(false, costs) {}
|
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 {
|
void computeForward(const InterfaceState& from) override {
|
||||||
SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost());
|
for (std::size_t i = 0; i < solutions_per_compute_; ++i) {
|
||||||
sendForward(from, InterfaceState(from.scene()->diff()), std::move(solution));
|
SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost());
|
||||||
|
sendForward(from, InterfaceState(from.scene()->diff()), std::move(solution));
|
||||||
|
}
|
||||||
}
|
}
|
||||||
void computeBackward(const InterfaceState& to) override {
|
void computeBackward(const InterfaceState& to) override {
|
||||||
SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost());
|
for (std::size_t i = 0; i < solutions_per_compute_; ++i) {
|
||||||
sendBackward(InterfaceState(to.scene()->diff()), to, std::move(solution));
|
SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost());
|
||||||
|
sendBackward(InterfaceState(to.scene()->diff()), to, std::move(solution));
|
||||||
|
}
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
class ForwardMockup : public PropagatorMockup
|
class ForwardMockup : public PropagatorMockup
|
||||||
@ -75,7 +81,8 @@ class ForwardMockup : public PropagatorMockup
|
|||||||
static unsigned int id_;
|
static unsigned int id_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
ForwardMockup(std::initializer_list<double> costs = { 0.0 }) : PropagatorMockup(costs) {
|
ForwardMockup(std::initializer_list<double> costs = { 0.0 }, std::size_t solutions_per_compute = 1)
|
||||||
|
: PropagatorMockup(costs, solutions_per_compute) {
|
||||||
restrictDirection(FORWARD);
|
restrictDirection(FORWARD);
|
||||||
setName("FW" + std::to_string(++id_));
|
setName("FW" + std::to_string(++id_));
|
||||||
}
|
}
|
||||||
@ -95,7 +102,7 @@ public:
|
|||||||
struct ForwardDummy : PropagatingForward
|
struct ForwardDummy : PropagatingForward
|
||||||
{
|
{
|
||||||
using PropagatingForward::PropagatingForward;
|
using PropagatingForward::PropagatingForward;
|
||||||
void computeForward(const InterfaceState& from) override {}
|
void computeForward(const InterfaceState& /*from*/) override {}
|
||||||
};
|
};
|
||||||
|
|
||||||
/* Connect creating solutions with given costs */
|
/* Connect creating solutions with given costs */
|
||||||
@ -176,6 +183,28 @@ TEST(ConnectConnect, PruningForward) {
|
|||||||
EXPECT_EQ(c2->calls_, 6u);
|
EXPECT_EQ(c2->calls_, 6u);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
TEST(ConnectConnect, PruningMultiForward) {
|
||||||
|
GeneratorMockup::id_ = Connect::id_ = 0; // reset IDs
|
||||||
|
Task t;
|
||||||
|
|
||||||
|
t.setRobotModel(getModel());
|
||||||
|
t.add(Stage::pointer(new GeneratorMockup()));
|
||||||
|
t.add(Stage::pointer(new Connect()));
|
||||||
|
t.add(Stage::pointer(new BackwardMockup()));
|
||||||
|
/* TODO(v4hn):
|
||||||
|
* t.add(Stage::pointer(new GeneratorMockup()));
|
||||||
|
* should be enough instead of the above, but propagators do not even respect ENABLED/DISABLED yet
|
||||||
|
*/
|
||||||
|
|
||||||
|
t.add(Stage::pointer(new GeneratorMockup()));
|
||||||
|
t.add(Stage::pointer(new ForwardMockup({ 0, 0 }, 2))); // spawn two solutions for the only incoming state
|
||||||
|
t.add(Stage::pointer(new ForwardMockup({ 0, inf })));
|
||||||
|
t.plan();
|
||||||
|
|
||||||
|
ASSERT_EQ(t.solutions().size(), 1);
|
||||||
|
EXPECT_EQ((*t.solutions().begin())->cost(), 0u);
|
||||||
|
}
|
||||||
|
|
||||||
TEST(ConnectConnect, PruningBackward) {
|
TEST(ConnectConnect, PruningBackward) {
|
||||||
GeneratorMockup::id_ = Connect::id_ = 0; // reset IDs
|
GeneratorMockup::id_ = Connect::id_ = 0; // reset IDs
|
||||||
Task t;
|
Task t;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user