mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
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:
parent
d6714e89ab
commit
932497ce6d
@ -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)
|
||||
|
||||
|
@ -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)
|
||||
|
@ -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();
|
||||
|
Loading…
Reference in New Issue
Block a user