diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index aa394841f..3392b3208 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -180,6 +180,8 @@ class Stage const std::string& name() const; void setName(const std::string& name); + uint32_t introspectionId() const; + /** set computation timeout (in seconds) * * The logic of the individual stage should ensure this limit is respected. diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 61ab03c33..990c85680 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -283,6 +283,12 @@ void Stage::setName(const std::string& name) { pimpl_->name_ = name; } +uint32_t Stage::introspectionId() const { + if (!pimpl_->introspection_) + throw std::runtime_error("Task is not initialized yet or Introspection was disabled."); + return const_cast(pimpl_->introspection_)->stageId(this); +} + void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest) { const PropertyMap& src = source.properties(); PropertyMap& dst = dest.properties();