add missing reset()

This commit is contained in:
Robert Haschke 2018-03-24 11:02:03 +01:00
parent 76c6ba14fa
commit 603696f294
2 changed files with 8 additions and 1 deletions

View File

@ -47,6 +47,7 @@ class GeneratePose : public MonitoringGenerator {
public: public:
GeneratePose(const std::string& name); GeneratePose(const std::string& name);
void reset() override;
bool canCompute() const override; bool canCompute() const override;
bool compute() override; bool compute() override;
@ -57,7 +58,7 @@ public:
protected: protected:
void onNewSolution(const SolutionBase& s) override; void onNewSolution(const SolutionBase& s) override;
std::deque<planning_scene::PlanningSceneConstPtr> scenes_; std::deque<planning_scene::PlanningScenePtr> scenes_;
}; };
} } } } } }

View File

@ -53,6 +53,12 @@ GeneratePose::GeneratePose(const std::string& name)
p.declare<geometry_msgs::PoseStamped>("pose", "target pose to pass on in spawned states"); p.declare<geometry_msgs::PoseStamped>("pose", "target pose to pass on in spawned states");
} }
void GeneratePose::reset()
{
MonitoringGenerator::reset();
scenes_.clear();
}
void GeneratePose::onNewSolution(const SolutionBase& s) void GeneratePose::onNewSolution(const SolutionBase& s)
{ {
scenes_.push_back(s.end()->scene()->diff()); scenes_.push_back(s.end()->scene()->diff());