diff --git a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h index f81ee5d0..9f4b4885 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h @@ -47,6 +47,7 @@ class GenerateGraspPose : public MonitoringGenerator { public: GenerateGraspPose(const std::string& name); + void reset() override; bool canCompute() const override; bool compute() override; diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 53582637..6bd164a3 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -85,6 +85,13 @@ void GenerateGraspPose::setAngleDelta(double delta){ setProperty("angle_delta", delta); } +void GenerateGraspPose::reset() +{ + scene_.reset(); + current_angle_ = 0.0; + MonitoringGenerator::reset(); +} + void GenerateGraspPose::onNewSolution(const SolutionBase& s) { if (scene_)