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
|
||||
{
|
||||
PredefinedCosts costs_;
|
||||
std::size_t solutions_per_compute_;
|
||||
|
||||
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 {
|
||||
SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost());
|
||||
sendForward(from, InterfaceState(from.scene()->diff()), std::move(solution));
|
||||
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 {
|
||||
SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost());
|
||||
sendBackward(InterfaceState(to.scene()->diff()), to, std::move(solution));
|
||||
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));
|
||||
}
|
||||
}
|
||||
};
|
||||
class ForwardMockup : public PropagatorMockup
|
||||
@ -75,7 +81,8 @@ class ForwardMockup : public PropagatorMockup
|
||||
static unsigned int id_;
|
||||
|
||||
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);
|
||||
setName("FW" + std::to_string(++id_));
|
||||
}
|
||||
@ -95,7 +102,7 @@ public:
|
||||
struct ForwardDummy : PropagatingForward
|
||||
{
|
||||
using PropagatingForward::PropagatingForward;
|
||||
void computeForward(const InterfaceState& from) override {}
|
||||
void computeForward(const InterfaceState& /*from*/) override {}
|
||||
};
|
||||
|
||||
/* Connect creating solutions with given costs */
|
||||
@ -176,6 +183,28 @@ TEST(ConnectConnect, PruningForward) {
|
||||
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) {
|
||||
GeneratorMockup::id_ = Connect::id_ = 0; // reset IDs
|
||||
Task t;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user