Fix issues with Merger stage

This commit is contained in:
Robert Haschke 2020-09-23 08:00:42 +02:00
commit d3b878a31c

View File

@ -757,8 +757,8 @@ void Fallbacks::onNewSolution(const SolutionBase& s) {
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
void MergerPrivate::resolveInterface(InterfaceFlags expected) { void MergerPrivate::resolveInterface(InterfaceFlags expected) {
ContainerBasePrivate::resolveInterface(expected); ParallelContainerBasePrivate::resolveInterface(expected);
switch (interfaceFlags()) { switch (requiredInterface()) {
case PROPAGATE_FORWARDS: case PROPAGATE_FORWARDS:
case PROPAGATE_BACKWARDS: case PROPAGATE_BACKWARDS:
case UNKNOWN: case UNKNOWN:
@ -806,6 +806,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:
@ -822,8 +825,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;
} }
@ -862,18 +865,20 @@ void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) {
void MergerPrivate::sendForward(SubTrajectory&& t, const InterfaceState* from) { void MergerPrivate::sendForward(SubTrajectory&& t, const InterfaceState* from) {
// generate target state // generate target state
planning_scene::PlanningScenePtr to = from->scene()->diff(); planning_scene::PlanningScenePtr to = from->scene()->diff();
to->setCurrentState(t.trajectory()->getLastWayPoint()); if (t.trajectory() && !t.trajectory()->empty())
to->setCurrentState(t.trajectory()->getLastWayPoint());
StagePrivate::sendForward(*from, InterfaceState(to), std::make_shared<SubTrajectory>(std::move(t))); StagePrivate::sendForward(*from, InterfaceState(to), std::make_shared<SubTrajectory>(std::move(t)));
} }
void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) { void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) {
// generate target state // generate target state
planning_scene::PlanningScenePtr from = to->scene()->diff(); planning_scene::PlanningScenePtr from = to->scene()->diff();
from->setCurrentState(t.trajectory()->getFirstWayPoint()); if (t.trajectory() && !t.trajectory()->empty())
from->setCurrentState(t.trajectory()->getFirstWayPoint());
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
} }
@ -919,39 +924,46 @@ 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;
try { try {
merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg); merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg);
} catch (const std::runtime_error& e) { } catch (const std::runtime_error& e) {
ROS_INFO_STREAM_NAMED("Merger", this->name() << "Merging failed: " << e.what()); SubTrajectory t;
t.markAsFailure();
t.setComment(e.what());
spawner(std::move(t));
return; return;
} }
if (jmg_merged_.get() != jmg) if (jmg_merged_.get() != jmg)
jmg_merged_.reset(jmg); jmg_merged_.reset(jmg);
if (!merged)
return; assert(merged);
SubTrajectory t(merged);
// check merged trajectory for collisions // check merged trajectory for collisions
if (!start_scene->isPathValid(*merged)) std::vector<std::size_t> invalid_index;
return; if (!start_scene->isPathValid(*merged, "", true, &invalid_index)) {
t.markAsFailure();
SubTrajectory t(merged); std::ostringstream oss;
// accumulate costs and markers oss << "Invalid waypoint(s): ";
double costs = 0.0; if (invalid_index.size() == merged->getWayPointCount())
for (const auto& sub : sub_solutions) { oss << "all";
costs += sub->cost(); else for (size_t i : invalid_index)
t.markers().insert(t.markers().end(), sub->markers().begin(), sub->markers().end()); oss << i << ", ";
t.setComment(oss.str());
} else {
// accumulate costs and markers
double costs = 0.0;
for (const auto& sub : sub_solutions) {
costs += sub->cost();
t.markers().insert(t.markers().end(), sub->markers().begin(), sub->markers().end());
}
t.setCost(costs);
} }
t.setCost(costs);
spawner(std::move(t)); spawner(std::move(t));
} }
} // namespace task_constructor } // namespace task_constructor