expose Stage API to get the matching introspection ID

This was implicitly requested in #192 .

Author: v4hn

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.

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();