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) {}
void MergerPrivate::resolveInterface(InterfaceFlags expected) {
ContainerBasePrivate::resolveInterface(expected);
switch (interfaceFlags()) {
ParallelContainerBasePrivate::resolveInterface(expected);
switch (requiredInterface()) {
case PROPAGATE_FORWARDS:
case PROPAGATE_BACKWARDS:
case UNKNOWN:
@ -806,6 +806,9 @@ void Merger::compute() {
}
void Merger::onNewSolution(const SolutionBase& s) {
if (s.isFailure()) // ignore failure solutions
return;
auto impl = pimpl();
switch (impl->interfaceFlags()) {
case PROPAGATE_FORWARDS:
@ -822,8 +825,8 @@ void Merger::onNewSolution(const SolutionBase& s) {
void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) {
const SubTrajectory* trajectory = dynamic_cast<const SubTrajectory*>(&s);
if (!trajectory) {
ROS_ERROR_NAMED("Merger", "Only simple trajectories are supported");
if (!trajectory || !trajectory->trajectory()) {
ROS_ERROR_NAMED("Merger", "Only simple, valid trajectories are supported");
return;
}
@ -862,18 +865,20 @@ void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) {
void MergerPrivate::sendForward(SubTrajectory&& t, const InterfaceState* from) {
// generate target state
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)));
}
void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) {
// generate target state
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)));
}
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
}
@ -919,39 +924,46 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
// transform vector of SubTrajectories into vector of RobotTrajectories
std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories;
sub_trajectories.reserve(sub_solutions.size());
for (const auto& sub : sub_solutions) {
// TODO: directly skip failures in mergeAnyCombination() or even earlier
if (sub->isFailure())
return;
if (sub->trajectory())
sub_trajectories.push_back(sub->trajectory());
}
for (const auto& sub : sub_solutions)
sub_trajectories.push_back(sub->trajectory());
moveit::core::JointModelGroup* jmg = jmg_merged_.get();
robot_trajectory::RobotTrajectoryPtr merged;
try {
merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg);
} 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;
}
if (jmg_merged_.get() != jmg)
jmg_merged_.reset(jmg);
if (!merged)
return;
assert(merged);
SubTrajectory t(merged);
// check merged trajectory for collisions
if (!start_scene->isPathValid(*merged))
return;
SubTrajectory t(merged);
// 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());
std::vector<std::size_t> invalid_index;
if (!start_scene->isPathValid(*merged, "", true, &invalid_index)) {
t.markAsFailure();
std::ostringstream oss;
oss << "Invalid waypoint(s): ";
if (invalid_index.size() == merged->getWayPointCount())
oss << "all";
else for (size_t i : invalid_index)
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));
}
} // namespace task_constructor