expose Stage API to get the matching introspection ID

This was implicitly requested in moveit#192 .
v4hn · Aug 11, 2020 · 52e8d0a · 52e8d0a
1 parent 062a0c2
commit 52e8d0a
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 0 deletions.
diff --git 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
@@ -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<const moveit::task_constructor::Introspection*>(pimpl_->introspection_)->stageId(this);
+}
+
 void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest) {
 	const PropertyMap& src = source.properties();
 	PropertyMap& dst = dest.properties();