mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
add test to validate correct solution state in computeCost
This is somewhat cumbersome because of the additional internal/external layer introduced through the container. But it's still better than leaving this unverified.
This commit is contained in:
parent
f4fb8c6f71
commit
3d8faea079
@ -113,9 +113,13 @@ public:
|
||||
bool canCompute() const override;
|
||||
void compute() override;
|
||||
|
||||
// internal interface for first/last child to push to if required
|
||||
InterfacePtr pendingBackward() const { return pending_backward_; }
|
||||
InterfacePtr pendingForward() const { return pending_forward_; }
|
||||
|
||||
// map InterfaceStates from children to external InterfaceStates of the container
|
||||
const auto& internalToExternalMap() const { return internal_to_external_; }
|
||||
|
||||
protected:
|
||||
ContainerBasePrivate(ContainerBase* me, const std::string& name);
|
||||
|
||||
@ -138,7 +142,6 @@ protected:
|
||||
const InterfaceState* internal_to);
|
||||
|
||||
auto& internalToExternalMap() { return internal_to_external_; }
|
||||
const auto& internalToExternalMap() const { return internal_to_external_; }
|
||||
|
||||
// set in resolveInterface()
|
||||
InterfaceFlags required_interface_;
|
||||
|
||||
@ -98,12 +98,12 @@ class BackwardMockup : public PropagatingBackward
|
||||
template <typename T>
|
||||
class Standalone : public T
|
||||
{
|
||||
public:
|
||||
moveit::core::RobotModelConstPtr robot;
|
||||
InterfacePtr dummy;
|
||||
planning_scene::PlanningSceneConstPtr ps;
|
||||
InterfaceStatePtr state_start, state_end;
|
||||
|
||||
public:
|
||||
Standalone(const moveit::core::RobotModelConstPtr& robot)
|
||||
: T(), robot(robot), dummy(std::make_shared<Interface>()), ps(new planning_scene::PlanningScene(robot)) {}
|
||||
|
||||
@ -112,8 +112,8 @@ public:
|
||||
auto impl{ this->pimpl() };
|
||||
this->reset();
|
||||
|
||||
state_start.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0)));
|
||||
state_end.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0)));
|
||||
state_start.reset();
|
||||
state_end.reset();
|
||||
|
||||
// infer and setup interface from children
|
||||
Stage* s{ this };
|
||||
@ -137,9 +137,11 @@ public:
|
||||
|
||||
// feed interfaces as required for one computation
|
||||
if (flags & READS_START) {
|
||||
state_start.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0)));
|
||||
impl->starts()->add(*state_start);
|
||||
}
|
||||
if (flags & READS_END) {
|
||||
state_end.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0)));
|
||||
impl->ends()->add(*state_end);
|
||||
}
|
||||
}
|
||||
@ -171,6 +173,39 @@ public:
|
||||
}
|
||||
};
|
||||
|
||||
TEST(CostTerm, SolutionConnected) {
|
||||
const moveit::core::RobotModelConstPtr robot{ getModel() };
|
||||
|
||||
Standalone<SerialContainer> container(robot);
|
||||
auto stage{ std::make_unique<ConnectMockup>() };
|
||||
|
||||
// custom CostTerm to verify SubTrajectory is hooked up to its states & creator
|
||||
class VerifySolutionCostTerm : public TrajectoryCostTerm
|
||||
{
|
||||
Standalone<SerialContainer>& container_;
|
||||
Stage* creator_;
|
||||
|
||||
public:
|
||||
VerifySolutionCostTerm(Standalone<SerialContainer>& container, Stage* creator)
|
||||
: container_{ container }, creator_{ creator } {}
|
||||
|
||||
double operator()(const SubTrajectory& s, std::string& /*comment*/) const override {
|
||||
EXPECT_EQ(&*container_.state_start,
|
||||
const_cast<const SerialContainerPrivate*>(container_.pimpl())->internalToExternalMap().at(s.start()))
|
||||
<< "SubTrajectory is not connected to its expected start InterfaceState";
|
||||
EXPECT_EQ(&*container_.state_end,
|
||||
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;
|
||||
}
|
||||
};
|
||||
|
||||
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";
|
||||
}
|
||||
|
||||
TEST(CostTerm, SetLambdaCostTerm) {
|
||||
const moveit::core::RobotModelConstPtr robot{ getModel() };
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user