Reject failures early

This commit is contained in:
Robert Haschke 2020-07-11 16:54:40 +02:00
parent 8152994e52
commit 14fc3d2586

View File

@ -805,6 +805,9 @@ void Merger::compute() {
} }
void Merger::onNewSolution(const SolutionBase& s) { void Merger::onNewSolution(const SolutionBase& s) {
if (s.isFailure()) // ignore failure solutions
return;
auto impl = pimpl(); auto impl = pimpl();
switch (impl->interfaceFlags()) { switch (impl->interfaceFlags()) {
case PROPAGATE_FORWARDS: case PROPAGATE_FORWARDS:
@ -821,8 +824,8 @@ void Merger::onNewSolution(const SolutionBase& s) {
void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) { void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) {
const SubTrajectory* trajectory = dynamic_cast<const SubTrajectory*>(&s); const SubTrajectory* trajectory = dynamic_cast<const SubTrajectory*>(&s);
if (!trajectory) { if (!trajectory || !trajectory->trajectory()) {
ROS_ERROR_NAMED("Merger", "Only simple trajectories are supported"); ROS_ERROR_NAMED("Merger", "Only simple, valid trajectories are supported");
return; return;
} }
@ -872,7 +875,7 @@ void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) {
StagePrivate::sendBackward(InterfaceState(from), *to, std::make_shared<SubTrajectory>(std::move(t))); StagePrivate::sendBackward(InterfaceState(from), *to, std::make_shared<SubTrajectory>(std::move(t)));
} }
void MergerPrivate::onNewGeneratorSolution(const SolutionBase& s) { void MergerPrivate::onNewGeneratorSolution(const SolutionBase& /* s */) {
// TODO: implement in similar fashion as onNewPropagateSolution(), but also merge start/end states // TODO: implement in similar fashion as onNewPropagateSolution(), but also merge start/end states
} }
@ -918,13 +921,8 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
// transform vector of SubTrajectories into vector of RobotTrajectories // transform vector of SubTrajectories into vector of RobotTrajectories
std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories; std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories;
sub_trajectories.reserve(sub_solutions.size()); sub_trajectories.reserve(sub_solutions.size());
for (const auto& sub : sub_solutions) { for (const auto& sub : sub_solutions)
// TODO: directly skip failures in mergeAnyCombination() or even earlier sub_trajectories.push_back(sub->trajectory());
if (sub->isFailure())
return;
if (sub->trajectory())
sub_trajectories.push_back(sub->trajectory());
}
moveit::core::JointModelGroup* jmg = jmg_merged_.get(); moveit::core::JointModelGroup* jmg = jmg_merged_.get();
robot_trajectory::RobotTrajectoryPtr merged; robot_trajectory::RobotTrajectoryPtr merged;