diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 9e41d409..7f1246b3 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -65,7 +65,7 @@ std::string getTaskId(const TaskPrivate* task) { class IntrospectionPrivate { public: - IntrospectionPrivate(const TaskPrivate* task) + IntrospectionPrivate(const TaskPrivate* task, Introspection* self) : nh_(std::string("~/") + task->ns()) // topics + services are advertised in private namespace , task_(task) , task_id_(getTaskId(task)) { @@ -78,8 +78,11 @@ public: nh_.advertise(STATISTICS_TOPIC, 1, true); solution_publisher_ = nh_.advertise(SOLUTION_TOPIC, 1, true); + get_solution_service_ = nh_.advertiseService(GET_SOLUTION_SERVICE, &Introspection::getSolution, self); + resetMaps(); } + ~IntrospectionPrivate() { indicateReset(); } void indicateReset() { // send empty task description message to indicate reset @@ -114,9 +117,7 @@ public: boost::bimap id_solution_bimap_; }; -Introspection::Introspection(const TaskPrivate* task) : impl(new IntrospectionPrivate(task)) { - impl->get_solution_service_ = impl->nh_.advertiseService(GET_SOLUTION_SERVICE, &Introspection::getSolution, this); -} +Introspection::Introspection(const TaskPrivate* task) : impl(new IntrospectionPrivate(task, this)) {} Introspection::~Introspection() { delete impl; diff --git a/core/src/task.cpp b/core/src/task.cpp index 9d61dc7f..44318346 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -173,6 +173,7 @@ planning_pipeline::PlanningPipelinePtr Task::createPlanner(const robot_model::Ro Task::~Task() { auto impl = pimpl(); + impl->introspection_.reset(); // stop introspection clear(); // remove all stages impl->robot_model_.reset(); // only destroy loader after all references to the model are gone!