do not pass failures on to monitors

They should not work with them and this
removes the need for the usual `if(s.isFailure) return`.
This commit is contained in:
v4hn 2018-09-23 16:51:05 +02:00 committed by Robert Haschke
parent d6714e89ab
commit 932497ce6d
3 changed files with 12 additions and 4 deletions

View File

@ -217,12 +217,16 @@ PIMPL_FUNCTIONS(Generator)
class MonitoringGeneratorPrivate : public GeneratorPrivate {
friend class MonitoringGenerator;
public:
Stage* monitored_;
Stage::SolutionCallbackList::const_iterator cb_;
bool registered_;
inline MonitoringGeneratorPrivate(MonitoringGenerator *me, const std::string &name);
private:
void solutionCB(const SolutionBase &s);
};
PIMPL_FUNCTIONS(MonitoringGenerator)

View File

@ -614,11 +614,18 @@ void MonitoringGenerator::init(const moveit::core::RobotModelConstPtr& robot_mod
if (!impl->monitored_)
throw InitStageException(*this, "no monitored stage defined");
if (!impl->registered_) { // register only once
impl->cb_ = impl->monitored_->addSolutionCallback(std::bind(&MonitoringGenerator::onNewSolution, this, std::placeholders::_1));
impl->cb_ = impl->monitored_->addSolutionCallback(std::bind(&MonitoringGeneratorPrivate::solutionCB, impl, std::placeholders::_1));
impl->registered_ = true;
}
}
void MonitoringGeneratorPrivate::solutionCB(const SolutionBase &s)
{
// forward only successful solutions to monitor
if(!s.isFailure())
static_cast<MonitoringGenerator*>(me())->onNewSolution(s);
}
ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
: ComputeBasePrivate(me, name)

View File

@ -200,9 +200,6 @@ void ComputeIK::init(const moveit::core::RobotModelConstPtr& robot_model)
void ComputeIK::onNewSolution(const SolutionBase &s)
{
if (s.isFailure())
return;
assert(s.start() && s.end());
assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator
planning_scene::PlanningScenePtr sandbox_scene = s.start()->scene()->diff();