From d0b9890e0fe609a70dba6402ae8103ef6016a654 Mon Sep 17 00:00:00 2001
From: leoja464 <leoja464@student.liu.se>
Date: Tue, 10 Sep 2024 08:08:41 +0200
Subject: [PATCH] added some code over from dyknow ros 1, but there are some
 errors to fix

---
 .devcontainer/devcontainer.json               |   4 +-
 .vscode/c_cpp_properties.json                 |   5 +
 .vscode/settings.json                         |  10 +-
 .../include/dyknow_echo/echoComposition.hpp   |   4 +-
 src/dyknow_manager/CMakeLists.txt             |  25 +-
 .../dyknow_manager/compunit_helper.hpp        |  33 ++
 .../dyknow_manager/configuration_manager.hpp  |  71 +--
 .../dyknow_manager/configuration_node.hpp     |  48 ++
 .../dyknow_manager/configuration_planner.hpp  | 112 ++++
 .../dyknow_manager/feasibility_matrix.hpp     |  72 +++
 .../dyknow_manager/manager_componet.hpp       |   4 +-
 .../include/dyknow_manager/target_spec.hpp    |   2 +-
 .../dyknow_manager/transformation_spec.hpp    |   2 +-
 src/dyknow_manager/src/compunit_helper.cpp    |  88 +++
 .../src/configuration_manager.cpp             |  64 +-
 src/dyknow_manager/src/configuration_node.cpp |  57 ++
 .../src/configuration_planner.cpp             | 551 ++++++++++++++++++
 src/dyknow_manager/src/environment.cpp        |   2 +-
 src/dyknow_manager/src/feasibility_matrix.cpp |  40 ++
 src/dyknow_manager/src/manager_componet.cpp   |   2 +-
 src/dyknow_manager/srv/ExpPopulate.srv        |   9 +-
 src/dyknow_nodehandle/src/node.cpp            |   1 -
 22 files changed, 1112 insertions(+), 94 deletions(-)
 create mode 100644 src/dyknow_manager/include/dyknow_manager/compunit_helper.hpp
 create mode 100644 src/dyknow_manager/include/dyknow_manager/configuration_node.hpp
 create mode 100644 src/dyknow_manager/include/dyknow_manager/configuration_planner.hpp
 create mode 100644 src/dyknow_manager/include/dyknow_manager/feasibility_matrix.hpp
 create mode 100644 src/dyknow_manager/src/compunit_helper.cpp
 create mode 100644 src/dyknow_manager/src/configuration_node.cpp
 create mode 100644 src/dyknow_manager/src/configuration_planner.cpp
 create mode 100644 src/dyknow_manager/src/feasibility_matrix.cpp

diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json
index f7f16a1..b0cf01c 100644
--- a/.devcontainer/devcontainer.json
+++ b/.devcontainer/devcontainer.json
@@ -32,8 +32,10 @@
             }
         }
     },
+    "workspaceMount": "src=${localWorkspaceFolder},dst=/root/ros2_ws,type=bind",
+    //"workspaceMount": "src=/home/leolol/Code/dyknow-ros-2,dst=/root/ros2_ws,type=volume",
     "workspaceFolder": "/root/ros2_ws",
-    "workspaceMount": "source=${localWorkspaceFolder},target=/root/ros2_ws,type=bind",
+    //"mounts": ["src=/home/leolol/Code/dyknow-ros-2,dst=/root/ros2_ws,type=bind"],
     "mounts": [],
     "runArgs": [
         "--net=host"
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index bfc060a..9ca9749 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -6,9 +6,14 @@
         "limitSymbolsToIncludedHeaders": false
       },
       "includePath": [
+        "/root/ros2_ws/install/dyknow_manager/include/**",
+        "/root/ros2_ws/install/dyknow_echo/include/**",
+        "/root/ros2_ws/install/dyknow_nodehandle/include/**",
+        "/root/ros2_ws/install/dyknow_analytics/include/**",
         "/opt/ros/jazzy/include/**",
         "/root/ros2_ws/src/dyknow_analytics/include/**",
         "/root/ros2_ws/src/dyknow_echo/include/**",
+        "/root/ros2_ws/src/dyknow_manager/include/**",
         "/root/ros2_ws/src/dyknow_nodehandle/include/**",
         "/usr/include/**"
       ],
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 8a75380..8130f95 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,8 +1,14 @@
 {
     "python.autoComplete.extraPaths": [
+        "/root/ros2_ws/install/dyknow_manager/lib/python3.12/site-packages",
+        "/root/ros2_ws/install/dyknow_nodehandle/lib/python3.12/site-packages",
+        "/root/ros2_ws/install/dyknow_analytics/lib/python3.12/site-packages",
         "/opt/ros/jazzy/lib/python3.12/site-packages"
     ],
     "python.analysis.extraPaths": [
+        "/root/ros2_ws/install/dyknow_manager/lib/python3.12/site-packages",
+        "/root/ros2_ws/install/dyknow_nodehandle/lib/python3.12/site-packages",
+        "/root/ros2_ws/install/dyknow_analytics/lib/python3.12/site-packages",
         "/opt/ros/jazzy/lib/python3.12/site-packages"
     ],
     "files.associations": {
@@ -89,5 +95,7 @@
         "typeindex": "cpp",
         "typeinfo": "cpp",
         "variant": "cpp"
-    }
+    },
+    "cmake.ignoreCMakeListsMissing": true,
+    "terminal.integrated.scrollback": 10000
 }
\ No newline at end of file
diff --git a/src/dyknow_echo/include/dyknow_echo/echoComposition.hpp b/src/dyknow_echo/include/dyknow_echo/echoComposition.hpp
index 2926ddd..907be28 100644
--- a/src/dyknow_echo/include/dyknow_echo/echoComposition.hpp
+++ b/src/dyknow_echo/include/dyknow_echo/echoComposition.hpp
@@ -3,7 +3,6 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <dyknow_nodehandle/node.hpp>
-//#include <dyknow_nodehandle/node_handle.hpp>
 #include <dyknow_nodehandle/msg/sample.hpp>
 #include <memory>
 
@@ -15,8 +14,7 @@ class EchoComp {
 	    void callback(std::shared_ptr<dyknow_nodehandle::msg::Sample>& sample);
 
 
-		rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-  		get_node_base_interface() const;
+		rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const;
 
 	private:
 		dyknow::Publisher pub;
diff --git a/src/dyknow_manager/CMakeLists.txt b/src/dyknow_manager/CMakeLists.txt
index 227391e..252751d 100644
--- a/src/dyknow_manager/CMakeLists.txt
+++ b/src/dyknow_manager/CMakeLists.txt
@@ -9,15 +9,14 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
-
 find_package(composition_interfaces REQUIRED)  
-
 find_package(dyknow_nodehandle REQUIRED)
 find_package(dyknow_analytics REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 
 
+ament_export_dependencies(rosidl_default_runtime)
 
-find_package(rosidl_default_generators REQUIRED)
 
 # Add message and service files
 rosidl_generate_interfaces(${PROJECT_NAME}
@@ -41,9 +40,9 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/Refresh.srv"
   "srv/Repair.srv"
   "srv/GetValidInputs.srv"
+  "srv/ExpPopulate.srv"
   DEPENDENCIES dyknow_nodehandle
 )
-ament_export_dependencies(rosidl_default_runtime)
 
 
 include_directories(
@@ -55,29 +54,32 @@ include_directories(
   "${CMAKE_BINARY_DIR}/rosidl_generator_cpp"
 )
 
-add_library(${PROJECT_NAME}_lib
+add_library(${PROJECT_NAME}_lib SHARED
    src/manager_componet.cpp
    #src/configuration_manager.cpp
-   )
+)
+
+target_include_directories(${PROJECT_NAME}_lib PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
 
 ament_target_dependencies(${PROJECT_NAME}_lib
   rclcpp
   rclcpp_components
   dyknow_nodehandle
-  )
+)
 
 rclcpp_components_register_node(${PROJECT_NAME}_lib
   PLUGIN "manager::ManagerComponet"
   EXECUTABLE dyknow_manager_componet
 )
 
-#target_include_directories(dyknow_manager_componet PUBLIC
-#  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-#  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+
 #target_compile_features(dyknow_manager_componet PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
 
 # Ensure that the generated message headers are available to this target
-add_dependencies(${PROJECT_NAME}_lib ${PROJECT_NAME}__rosidl_typesupport_cpp)
+# add_dependencies(${PROJECT_NAME}_lib ${PROJECT_NAME}__rosidl_typesupport_cpp)
 
 
 install(TARGETS ${PROJECT_NAME}_lib
@@ -91,4 +93,5 @@ install(DIRECTORY include/
     DESTINATION include/${PROJECT_NAME}/
 )
 
+
 ament_package()
diff --git a/src/dyknow_manager/include/dyknow_manager/compunit_helper.hpp b/src/dyknow_manager/include/dyknow_manager/compunit_helper.hpp
new file mode 100644
index 0000000..9ca5182
--- /dev/null
+++ b/src/dyknow_manager/include/dyknow_manager/compunit_helper.hpp
@@ -0,0 +1,33 @@
+#ifndef INCLUDE_COMPUNIT_HELPER_HPP_
+#define INCLUDE_COMPUNIT_HELPER_HPP_
+
+//#include <ros/ros.h>
+//#include <nodelet/nodelet.h>
+//#include "nodelet/NodeletLoad.h"
+//#include "nodelet/NodeletUnload.h"
+//#include "dyknow_nodehandle/node_handle.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+
+
+#include "dyknow_nodehandle/node.hpp"
+#include "transformation_spec.hpp"
+
+namespace dyknow {
+
+class CompUnitHelper {
+public:
+	CompUnitHelper() {}
+	~CompUnitHelper() {}
+	static bool spawn(std::shared_ptr<dyknow::Node> nh, const std::string& manager, const std::string& name, const std::string& source, std::vector<std::string>& args);
+	static bool configure(std::shared_ptr<dyknow::Node> nh, const std::string& manager, std::string name, std::map<std::string, std::string> inputMap, std::map<std::string, std::string> outputMap);
+	static bool destroy(std::shared_ptr<dyknow::Node> nh, const std::string& manager, const std::string& name);
+};
+
+
+} //namespace
+
+
+
+#endif /* INCLUDE_COMPUNIT_HELPER_HPP_ */
diff --git a/src/dyknow_manager/include/dyknow_manager/configuration_manager.hpp b/src/dyknow_manager/include/dyknow_manager/configuration_manager.hpp
index 8eebcad..e9dd91a 100644
--- a/src/dyknow_manager/include/dyknow_manager/configuration_manager.hpp
+++ b/src/dyknow_manager/include/dyknow_manager/configuration_manager.hpp
@@ -11,22 +11,22 @@
 #include "target_spec.hpp"
 
 #include "dyknow_nodehandle/node.hpp"
-#include "dyknow_manager/srv/GetConfiguration.hpp"
-#include "dyknow_manager/srv/AddTransformation.hpp"
-#include "dyknow_manager/srv/RemoveTransformation.hpp"
-#include "dyknow_manager/srv/Spawn.hpp"
-#include "dyknow_manager/srv/Destroy.hpp"
-#include "dyknow_manager/srv/AddTarget.hpp"
-#include "dyknow_manager/srv/RemoveTarget.hpp"
-#include "dyknow_manager/srv/Refresh.hpp"
-#include "dyknow_manager/srv/Repair.hpp"
-#include "dyknow_manager/srv/ExpPopulate.hpp"
-#include "dyknow_manager/srv/Unit.hpp"
-#include "dyknow_manager/srv/Transformation.hpp"
-#include "dyknow_manager/srv/Configure.hpp"
-#include "dyknow_manager/srv/Connection.hpp"
-#include "dyknow_manager/srv/Target.hpp"
-#include "dyknow_manager/srv/GetValidInputs.hpp"
+#include "dyknow_manager/srv/get_configuration.hpp"
+#include "dyknow_manager/srv/add_transformation.hpp"
+#include "dyknow_manager/srv/remove_transformation.hpp"
+#include "dyknow_manager/srv/spawn.hpp"
+#include "dyknow_manager/srv/destroy.hpp"
+#include "dyknow_manager/srv/add_target.hpp"
+#include "dyknow_manager/srv/remove_target.hpp"
+#include "dyknow_manager/srv/refresh.hpp"
+#include "dyknow_manager/srv/repair.hpp"
+#include "dyknow_manager/srv/exp_populate.hpp"
+#include "dyknow_manager/msg/unit.hpp"
+#include "dyknow_manager/msg/transformation.hpp"
+#include "dyknow_manager/srv/configure.hpp"
+#include "dyknow_manager/msg/connection.hpp"
+#include "dyknow_manager/msg/target.hpp"
+#include "dyknow_manager/srv/get_valid_inputs.hpp"
 
 namespace dyknow {
 
@@ -36,9 +36,11 @@ public:
 	static const unsigned int VERBOSITY = 10;
 	static const bool EXPERIMENTAL = true;
 
+	//, getConfigurationService(nh->create_service<dyknow_manager::srv::GetConfiguration>("get_model", &ConfigurationManager::getConfigurationCallback))
+	//, getConfigurationService(nh->create_service<dyknow_manager::srv::GetConfiguration>("get_model", [this](const std::shared_ptr<dyknow_manager::srv::GetConfiguration::Request> req, std::shared_ptr<dyknow_manager::srv::GetConfiguration::Response> res) {this->getConfigurationCallback(req, res);}))
 	ConfigurationManager(std::shared_ptr<dyknow::Node>& nh)
 	: nh(nh)
-	, getConfigurationService(nh->create_service<dyknow_manager::srv::GetConfiguration>("get_model", &ConfigurationManager::getConfigurationCallback))
+	, getConfigurationService(nh->create_service<dyknow_manager::srv::GetConfiguration>("get_model", std::bind(&ConfigurationManager::getConfigurationCallback, this, std::placeholder::_1, std::placeholder_2))) // We need to say where the function should operate i.e. this
 	, addTransformationService(nh->create_service<dyknow_manager::srv::AddTransformation>("add_transformation", &ConfigurationManager::addTransformationCallback))
 	, removeTransformationService(nh->create_service<dyknow_manager::srv::RemoveTransformation>("remove_transformation", &ConfigurationManager::removeTransformationCallback))
 	, spawnService(nh->create_service<dyknow_manager::srv::Spawn>("spawn", &ConfigurationManager::spawnCallback))
@@ -49,8 +51,9 @@ public:
 	, refreshService(nh->create_service<dyknow_manager::srv::Refresh>("refresh", &ConfigurationManager::refreshCallback))
 	, repairService(nh->create_service<dyknow_manager::srv::Repair>("repair", &ConfigurationManager::repairCallback))
 	, getValidInputsService(nh->create_service<dyknow_manager::srv::GetValidInputs>("get_valid_inputs", &ConfigurationManager::getValidInputsCallback))
-	{
-		processThread = std::shared_ptr< std::thread >(new std::thread(std::bind(&ConfigurationManager::spin)));
+	, planner(nh)
+	{						
+		this->processThread = std::shared_ptr< std::thread >(new std::thread(std::bind(&ConfigurationManager::spin)));
         /*
 		if(EXPERIMENTAL) {
 			createEnvironment(0, "/nodelet_manager");
@@ -63,18 +66,18 @@ public:
 	void createEnvironment(int id, std::string manager);
 
 	// Services
-	bool getConfigurationCallback(dyknow_manager::GetConfiguration::Request& req, dyknow_manager::GetConfiguration::Response& res);
-	bool addTransformationCallback(dyknow_manager::AddTransformation::Request& req, dyknow_manager::AddTransformation::Response& res);
-	bool removeTransformationCallback(dyknow_manager::RemoveTransformation::Request& req, dyknow_manager::RemoveTransformation::Response& res);
-	bool spawnCallback(dyknow_manager::Spawn::Request& req, dyknow_manager::Spawn::Response& res);
-	bool destroyCallback(dyknow_manager::Destroy::Request& req, dyknow_manager::Destroy::Response& res);
-	bool configureCallback(dyknow_manager::Configure::Request& req, dyknow_manager::Configure::Response& res);
-	bool addTargetCallback(dyknow_manager::AddTarget::Request& req, dyknow_manager::AddTarget::Response& res);
-	bool removeTargetCallback(dyknow_manager::RemoveTarget::Request& req, dyknow_manager::RemoveTarget::Response& res);
-	bool refreshCallback(dyknow_manager::Refresh::Request& req, dyknow_manager::Refresh::Response& res);
-	bool repairCallback(dyknow_manager::Repair::Request& req, dyknow_manager::Repair::Response& res);
-	bool getValidInputsCallback(dyknow_manager::GetValidInputs::Request& req, dyknow_manager::GetValidInputs::Response& res);
-	bool expPopulateCallback(dyknow_manager::ExpPopulate::Request& req, dyknow_manager::ExpPopulate::Response& res);
+	void getConfigurationCallback(const std::shared_ptr<dyknow_manager::srv::GetConfiguration::Request> req, 				std::shared_ptr<dyknow_manager::srv::GetConfiguration::Response> res);
+	void addTransformationCallback(const std::shared_ptr<dyknow_manager::srv::AddTransformation::Request> req, 			std::shared_ptr<dyknow_manager::srv::AddTransformation::Response> res);
+	void removeTransformationCallback(const std::shared_ptr<dyknow_manager::srv::RemoveTransformation::Request> req, 		std::shared_ptr<dyknow_manager::srv::RemoveTransformation::Response> res);
+	void spawnCallback(const std::shared_ptr<dyknow_manager::srv::Spawn::Request> req, 									std::shared_ptr<dyknow_manager::srv::Spawn::Response> res);
+	void destroyCallback(const std::shared_ptr<dyknow_manager::srv::Destroy::Request> req, 								std::shared_ptr<dyknow_manager::srv::Destroy::Response> res);
+	void configureCallback(const std::shared_ptr<dyknow_manager::srv::Configure::Request> req, 							std::shared_ptr<dyknow_manager::srv::Configure::Response> res);
+	void addTargetCallback(const std::shared_ptr<dyknow_manager::srv::AddTarget::Request> req, 							std::shared_ptr<dyknow_manager::srv::AddTarget::Response> res);
+	void removeTargetCallback(const std::shared_ptr<dyknow_manager::srv::RemoveTarget::Request> req, 						std::shared_ptr<dyknow_manager::srv::RemoveTarget::Response> res);
+	void refreshCallback(const std::shared_ptr<dyknow_manager::srv::Refresh::Request> req, 								std::shared_ptr<dyknow_manager::srv::Refresh::Response> res);
+	void repairCallback(const std::shared_ptr<dyknow_manager::srv::Repair::Request> req, 									std::shared_ptr<dyknow_manager::srv::Repair::Response> res);
+	void getValidInputsCallback(const std::shared_ptr<dyknow_manager::srv::GetValidInputs::Request> req, 					std::shared_ptr<dyknow_manager::srv::GetValidInputs::Response> res);
+	void expPopulateCallback(const std::shared_ptr<dyknow_manager::srv::ExpPopulate::Request> req, 						std::shared_ptr<dyknow_manager::srv::ExpPopulate::Response> res);
 
 	bool apply(Change delta, int environment);
 	void spin();
@@ -89,7 +92,7 @@ public:
 
 private:
 	std::shared_ptr<dyknow::Node> nh;
-	boost::shared_ptr< boost::thread > processThread;
+	std::shared_ptr< std::thread > processThread;
 
 	std::vector<Environment> environments;
 	std::queue<std::pair<int, Change> > changeQueue;
@@ -107,8 +110,8 @@ private:
     rclcpp::Service<dyknow_manager::srv::Repair>::SharedPtr repairService;
     rclcpp::Service<dyknow_manager::srv::GetValidInputs>::SharedPtr getValidInputsService;
 
-	boost::mutex deltaAccess;
-	boost::mutex envAccess;
+	std::mutex deltaAccess;
+	std::mutex envAccess;
 };
 
 
diff --git a/src/dyknow_manager/include/dyknow_manager/configuration_node.hpp b/src/dyknow_manager/include/dyknow_manager/configuration_node.hpp
new file mode 100644
index 0000000..c164594
--- /dev/null
+++ b/src/dyknow_manager/include/dyknow_manager/configuration_node.hpp
@@ -0,0 +1,48 @@
+#ifndef INCLUDE_CONFIGURATION_NODE_HPP_
+#define INCLUDE_CONFIGURATION_NODE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include "transformation_spec.hpp"
+#include "util/to_string.hpp"
+
+namespace dyknow {
+
+class ConfigurationNode {
+public:
+	ConfigurationNode* parentPtr;
+	TransformationSpec type;
+	std::map<int, std::vector<ConfigurationNode*> > optionMap;
+	std::map<int, ConfigurationNode*> bestMap;
+	double cost;
+	int arg;
+	int outPort;
+	double fwdCost;
+        static constexpr double MAX_COST = 100000;
+	bool future;
+
+	ConfigurationNode();
+	ConfigurationNode(ConfigurationNode* parentPtr, TransformationSpec type, int arg, int outPort, bool future = false);
+	~ConfigurationNode();
+
+	void addOption(ConfigurationNode* option, int port);
+	double getBestCostChain();
+
+//	bool hasType() { return this->type.type != UNKNOWN; }
+	bool hasBest(int port) { return bestMap.find(port) != bestMap.end() ? bestMap[port]->cost <= MAX_COST : false; }
+	bool hasOptions(int port) { return optionMap.find(port) != optionMap.end(); }
+	bool isFuture() { return future; }
+	bool isRoot() { return parentPtr == NULL; }
+
+	void print(int depth = 0) {
+		//ROS_INFO(<Need logger here as well>"%s %s <- %s/%s (%s)", std::string(depth, '\t').c_str(), ("_" + to_string(arg+1)).c_str(), type.label.c_str(), to_string(outPort+1).c_str(), type.source.c_str());
+		for (std::pair<int, ConfigurationNode*> option : bestMap) {
+			option.second->print(depth+1);
+		}
+	}
+};
+
+}
+
+
+
+#endif /* INCLUDE_CONFIGURATION_NODE_HPP_ */
diff --git a/src/dyknow_manager/include/dyknow_manager/configuration_planner.hpp b/src/dyknow_manager/include/dyknow_manager/configuration_planner.hpp
new file mode 100644
index 0000000..6845491
--- /dev/null
+++ b/src/dyknow_manager/include/dyknow_manager/configuration_planner.hpp
@@ -0,0 +1,112 @@
+#ifndef INCLUDE_CONFIGURATION_PLANNER_HPP_
+#define INCLUDE_CONFIGURATION_PLANNER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <queue>
+#include "util/to_string.hpp"
+#include "configuration_node.hpp"
+#include "change.hpp"
+#include "environment.hpp"
+#include "feasibility_matrix.hpp"
+#include "transformation_spec.hpp"
+#include "target_spec.hpp"
+
+
+#include "dyknow_nodehandle/node.hpp"
+#include "dyknow_analytics/monitor.hpp"
+#include "dyknow_analytics/analysis.hpp"
+
+namespace dyknow {
+
+class ConfigurationPlanner {
+public:
+	ConfigurationPlanner(std::shared_ptr<dyknow::Node>& nh) : nh(nh), expansionCount(0), prevBest(ConfigurationNode::MAX_COST+1) {	}
+	~ConfigurationPlanner() {}
+	Change optimise(Environment* envPtr, unsigned int verbosity = 0);
+	Change optimise(std::vector<TargetSpec> targets, ComputationGraph graph, FeasibilityMatrix matrix, std::vector<std::string> protections,
+			unsigned int verbosity = 0);
+	std::queue<ConfigurationNode*> expand(ConfigurationNode* nodePtr, ComputationGraph& graph, FeasibilityMatrix& matrix, unsigned int verbosity = 0);
+	void update(ConfigurationNode* nodePtr, FeasibilityMatrix& matrix, unsigned int verbosity = 0);
+	void getAncestors(ConfigurationNode* nodePtr, std::vector<TransformationSpec>& ancestors);
+	void generateChange(Change& change, ConfigurationNode* nodePtr, std::string outStream, ComputationGraph& graph, unsigned int verbosity = 0);
+	std::string createUniqueName(std::string base);
+
+	void expandCompUnit(ConfigurationNode* nodePtr, ComputationGraph& graph, FeasibilityMatrix& matrix, std::queue<ConfigurationNode*>& queue,
+			int index, std::vector<TransformationSpec> ancestors, std::vector<Port> candidates, bool& update, unsigned int verbosity = 0);
+	void expandTransformation(ConfigurationNode* nodePtr, FeasibilityMatrix& matrix,	std::queue<ConfigurationNode*>& queue, int index,
+			std::vector<TransformationSpec> ancestors, std::vector<Port> candidates, bool& update, unsigned int verbosity = 0);
+
+	void garbageCollect(Change& change, ComputationGraph& graph, unsigned int verbosity = 0);
+	void generateIndex(ConfigurationNode* nodePtr, unsigned int verbosity = 0);
+
+	void logPerformance(unsigned int verbosity = 0) {
+		if(expansionCount % (MAX_EXPANSIONS/100) == 0) {
+			if(verbosity >= 3)RCLCPP_INFO(nh->get_logger(), "Entering expansion size: %i", expansionCount);
+			expansionTime.push_back(nh->now()-startTime);
+		}
+	}
+
+	std::string concat(std::string label, std::string port) {
+		return label + "/" + port;
+	}
+
+	void reset() {
+		expansionCount = 0;
+		prevBest = ConfigurationNode::MAX_COST+1;
+		queue = std::queue<ConfigurationNode*>();
+		costIndex = std::vector<int>();
+		costChange = std::vector<double>();
+		depthExpansionIndex = std::vector<int>();
+		depthExpansionTime = std::vector<rclcpp::Duration>();
+		expansionTime = std::vector<rclcpp::Duration>();
+		costTime = std::vector<rclcpp::Duration>();
+		protections = std::vector<std::string>();
+		nodeIndex = std::map<std::string, ConfigurationNode*>();
+		portIndex = std::map<std::string, ConfigurationNode*>();
+//		futureMap = std::map<std::string, std::string>();
+		futurePublisherMap = std::map<std::string, std::string>();
+		futureNodeMap = std::map<std::string, std::string>();
+		expandCUTimes = std::vector<rclcpp::Duration>();
+		expandTFTimes = std::vector<rclcpp::Duration>();
+		expandTimes = std::vector<rclcpp::Duration>();
+		updateTimes = std::vector<rclcpp::Duration>();
+	}
+
+	int expansionCount;
+	rclcpp::Time startTime;
+	std::map<std::string, unsigned int> uniqueNameMap;
+	std::vector<std::string> protections;
+	std::map<std::string, ConfigurationNode*> nodeIndex;
+	std::map<std::string, ConfigurationNode*> portIndex;
+//	std::map<std::string, std::string> futureMap;
+	std::map<std::string, std::string> futurePublisherMap;
+	std::map<std::string, std::string> futureNodeMap;
+
+	std::queue<ConfigurationNode*> queue;
+	std::vector<int> costIndex;
+	std::vector<double> costChange;
+	std::vector<rclcpp::Duration> costTime;
+	std::vector<int> depthExpansionIndex;
+	std::vector<rclcpp::Duration> depthExpansionTime;
+	std::vector<rclcpp::Duration> expansionTime;
+
+	// Method performance
+	std::vector<rclcpp::Duration> expandCUTimes;
+	std::vector<rclcpp::Duration> expandTFTimes;
+	std::vector<rclcpp::Duration> expandTimes;
+	std::vector<rclcpp::Duration> updateTimes;
+
+        static constexpr int MAX_EXPANSIONS = 100000; //100000;//1000000;
+        static constexpr double MAX_TIME_SEC = 10;//3600; //60.0; //2.0
+
+	double prevBest;
+
+private:
+	std::shared_ptr<dyknow::Node> nh;
+
+};
+
+}
+
+
+#endif /* INCLUDE_CONFIGURATION_PLANNER_HPP_ */
diff --git a/src/dyknow_manager/include/dyknow_manager/feasibility_matrix.hpp b/src/dyknow_manager/include/dyknow_manager/feasibility_matrix.hpp
new file mode 100644
index 0000000..383d4f4
--- /dev/null
+++ b/src/dyknow_manager/include/dyknow_manager/feasibility_matrix.hpp
@@ -0,0 +1,72 @@
+#ifndef INCLUDE_FEASIBILITY_MATRIX_HPP_
+#define INCLUDE_FEASIBILITY_MATRIX_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include "transformation_spec.hpp"
+
+namespace dyknow {
+
+typedef std::pair<int, int> Port;
+
+class FeasibilityMatrixGenerator {
+public:
+	virtual ~FeasibilityMatrixGenerator() {}
+	virtual std::map<Port, std::vector<Port> > generate(std::vector<TransformationSpec> library) = 0;
+};
+
+class RandomMatrixGenerator : public FeasibilityMatrixGenerator {
+public:
+	RandomMatrixGenerator() : FeasibilityMatrixGenerator(), density(0.01) {}
+	RandomMatrixGenerator(double density) : FeasibilityMatrixGenerator(), density(density) {}
+	virtual ~RandomMatrixGenerator() {}
+	virtual std::map<Port, std::vector<Port> > generate(std::vector<TransformationSpec> library) {
+		int connections = 0;
+
+		std::map<Port, std::vector<Port> > result;
+		if(density == 0.0) { return result; }
+
+		for(TransformationSpec inSpec : library) {
+			for(int i = 0; i < inSpec.inPorts.size() ; i++) {
+				Port inPort(inSpec.id, i);
+				result[inPort] = std::vector<Port>();
+
+				for(TransformationSpec outSpec: library) {
+					for(int j = 0; j < outSpec.outPorts.size() ; j++) {
+						double value = (double)rand() / (double)RAND_MAX;
+						if(value <= density) {
+							result[inPort].push_back(Port(outSpec.id, j));
+							connections++;
+						}
+					}
+				}
+			}
+		}
+
+		//RCLCPP_INFO(<this is where i would put my logger, if I had one...> "Random matrix density: %f", ((double)connections/(double)(library.size()*library.size())));
+		return result;
+	}
+
+private:
+	double density;
+};
+
+class FeasibilityMatrix {
+public:
+	FeasibilityMatrix() {}
+	FeasibilityMatrix(std::vector<TransformationSpec> library, double density = 0.01);
+	~FeasibilityMatrix() {}
+
+	std::vector<Port> getValid(Port input);
+	TransformationSpec decode(Port port);
+	void invalidate(std::string transformation);
+
+	std::map<Port, std::vector<Port> > matrix;
+	std::map<std::pair<int, int>, std::pair<std::string, std::string> > decoder;
+	std::vector<TransformationSpec> library;
+};
+
+}
+
+
+
+#endif /* INCLUDE_FEASIBILITY_MATRIX_HPP_ */
diff --git a/src/dyknow_manager/include/dyknow_manager/manager_componet.hpp b/src/dyknow_manager/include/dyknow_manager/manager_componet.hpp
index f4343b5..f336019 100644
--- a/src/dyknow_manager/include/dyknow_manager/manager_componet.hpp
+++ b/src/dyknow_manager/include/dyknow_manager/manager_componet.hpp
@@ -3,7 +3,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <dyknow_nodehandle/node.hpp>
-//#include "configuration_manager.hpp"
+#include "configuration_manager.hpp"
 #include "rclcpp_components/component_manager.hpp"
 
 
@@ -17,7 +17,7 @@ class ManagerComponet {
 
     private:
         std::shared_ptr<dyknow::Node> node;
-        //std::shared_ptr<ConfigurationManager> configManPtr;
+        std::shared_ptr<dyknow::ConfigurationManager> configManPtr;
 
 };
 
diff --git a/src/dyknow_manager/include/dyknow_manager/target_spec.hpp b/src/dyknow_manager/include/dyknow_manager/target_spec.hpp
index 67c06d8..64b951d 100644
--- a/src/dyknow_manager/include/dyknow_manager/target_spec.hpp
+++ b/src/dyknow_manager/include/dyknow_manager/target_spec.hpp
@@ -34,7 +34,7 @@ struct TargetSpec {
 class TargetFactory {
 public:
 	static TargetSpec create(int id, std::string label, int port = 0, std::string topic = "/result");
-	static TargetSpec create(dyknow_manager::Target target);
+	static TargetSpec create(dyknow_manager::msg::Target target);
 };
 
 } //namespace
diff --git a/src/dyknow_manager/include/dyknow_manager/transformation_spec.hpp b/src/dyknow_manager/include/dyknow_manager/transformation_spec.hpp
index de23522..d529972 100644
--- a/src/dyknow_manager/include/dyknow_manager/transformation_spec.hpp
+++ b/src/dyknow_manager/include/dyknow_manager/transformation_spec.hpp
@@ -67,7 +67,7 @@ struct TransformationSpec {
 class TransformationFactory {
 public:
 	static TransformationSpec create(std::string path);
-	static TransformationSpec create(dyknow_manager::Transformation);
+	static TransformationSpec create(dyknow_manager::msg::Transformation);
 };
 
 } //namespace
diff --git a/src/dyknow_manager/src/compunit_helper.cpp b/src/dyknow_manager/src/compunit_helper.cpp
new file mode 100644
index 0000000..faeeb9b
--- /dev/null
+++ b/src/dyknow_manager/src/compunit_helper.cpp
@@ -0,0 +1,88 @@
+#include "compunit_helper.hpp"
+
+namespace dyknow {
+
+bool CompUnitHelper::spawn(dyknow::NodeHandle& nh, const std::string& manager, const std::string& name, const std::string& source, std::vector<std::string>& args) {
+	if(source == "") {
+		ROS_WARN("Failed to spawn: %s does not provide a source", name.c_str());
+		return false;
+	}
+
+	ros::ServiceClient client = nh.serviceClient<nodelet::NodeletLoad>(manager + "/load_nodelet");
+	nodelet::NodeletLoad loadNodeletService;
+	loadNodeletService.request.name = name;
+	loadNodeletService.request.type = source;
+	loadNodeletService.request.my_argv = args;
+
+	if (client.call(loadNodeletService))
+	{
+		ROS_INFO("Successfully spawned nodelet %s", name.c_str());
+		return true;
+	}
+	else
+	{
+		ROS_ERROR("Unable to call service for %s with source %s", name.c_str(), source.c_str());
+		return false;
+	}
+}
+
+bool CompUnitHelper::configure(dyknow::NodeHandle& nh, const std::string& manager, std::string name, std::map<std::string, std::string> inputMap, std::map<std::string, std::string> outputMap) {
+	ros::ServiceClient client = nh.serviceClient<dyknow_nodehandle::Configure>("/"+name+"/set_config");
+	dyknow_nodehandle::Configure configureService;
+
+	std::pair<std::string, std::string> elem;
+	BOOST_FOREACH(elem, outputMap) {
+		configureService.request.config.out_names.push_back(elem.second);
+		configureService.request.config.out_channels.push_back(elem.first);
+	}
+
+	BOOST_FOREACH(elem, inputMap) {
+		configureService.request.config.in_names.push_back(elem.second);
+		configureService.request.config.in_channels.push_back(elem.first);
+	}
+
+	configureService.request.config.manager = manager;
+
+	if (client.call(configureService))
+	{
+		if(!configureService.response.success) {
+			ROS_ERROR("Failed to configure nodelet %s", name.c_str());
+		}
+		else {
+			ROS_INFO("Successfully configured nodelet %s", name.c_str());
+		}
+		return true;
+	}
+	else
+	{
+		ROS_ERROR("Unable to configure nodelet %s", name.c_str());
+		return false;
+	}
+}
+
+bool CompUnitHelper::destroy(dyknow::NodeHandle& nh, const std::string& manager, const std::string& name) {
+	ros::ServiceClient unloadClient = nh.serviceClient<nodelet::NodeletUnload>("/" + manager + "/unload_nodelet");
+	nodelet::NodeletUnload unloadService;
+
+	// Call unload service
+	unloadService.request.name = name.c_str();
+
+	if (unloadClient.call(unloadService))
+	{
+		if(!unloadService.response.success) {
+			ROS_ERROR("Unable to unload nodelet %s: failure while unloading", name.c_str());
+			return false;
+		}
+		else {
+			ROS_INFO("Successfully unloaded nodelet %s", name.c_str());
+			return true;
+		}
+	}
+	else
+	{
+		ROS_ERROR("Unable to unload nodelet %s: service unavailable", name.c_str());
+		return false;
+	}
+}
+
+} //namespace
diff --git a/src/dyknow_manager/src/configuration_manager.cpp b/src/dyknow_manager/src/configuration_manager.cpp
index 324aab5..4b7add1 100644
--- a/src/dyknow_manager/src/configuration_manager.cpp
+++ b/src/dyknow_manager/src/configuration_manager.cpp
@@ -8,7 +8,7 @@ void ConfigurationManager::createEnvironment(int id, std::string manager) {
 	environments.push_back(env);
 }
 
-bool ConfigurationManager::getConfigurationCallback(dyknow_manager::GetConfiguration::Request& req, dyknow_manager::GetConfiguration::Response& res) {
+void ConfigurationManager::getConfigurationCallback(const std::shared_ptr<dyknow_manager::GetConfiguration::Request> req, std::shared_ptr<dyknow_manager::GetConfiguration::Response> res) {
 	std::lock_guard<std::mutex> guard(envAccess);
 	for (auto& env : environments) {
 		std::vector<ComputationUnit> computationUnits = env.getComputationUnits();
@@ -62,10 +62,10 @@ bool ConfigurationManager::getConfigurationCallback(dyknow_manager::GetConfigura
 			res.targets.push_back(tgt);
 		}
 	}
-	return true;
+	return;
 }
 
-bool ConfigurationManager::addTransformationCallback(dyknow_manager::AddTransformation::Request& req, dyknow_manager::AddTransformation::Response& res) {
+void ConfigurationManager::addTransformationCallback(const std::shared_ptr<dyknow_manager::AddTransformation::Request> req, std::shared_ptr<dyknow_manager::AddTransformation::Response> res) {
 	Change delta;
 	for (auto& tfSpec : req.transformations) {
 		TransformationSpec spec = TransformationFactory::create(tfSpec);
@@ -75,10 +75,10 @@ bool ConfigurationManager::addTransformationCallback(dyknow_manager::AddTransfor
 
 	enqueue(std::pair<int, Change>(req.envId, delta));
 	res.success = true;
-	return true;
+	return;
 }
 
-bool ConfigurationManager::removeTransformationCallback(dyknow_manager::RemoveTransformation::Request& req, dyknow_manager::RemoveTransformation::Response& res) {
+void ConfigurationManager::removeTransformationCallback(const std::shared_ptr<dyknow_manager::RemoveTransformation::Request> req, std::shared_ptr<dyknow_manager::RemoveTransformation::Response> res) {
 	Change delta;
 	for (auto& label : req.transformations) {
 		RemoveCommand* cmdPtr = new RemoveCommand(label);
@@ -87,33 +87,33 @@ bool ConfigurationManager::removeTransformationCallback(dyknow_manager::RemoveTr
 
 	enqueue(std::pair<int, Change>(req.envId, delta));
 	res.success = true;
-	return true;
+	return;
 }
 
-bool ConfigurationManager::spawnCallback(dyknow_manager::Spawn::Request& req, dyknow_manager::Spawn::Response& res) {
+void ConfigurationManager::spawnCallback(const std::shared_ptr<dyknow_manager::Spawn::Request> req, std::shared_ptr<dyknow_manager::Spawn::Response> res) {
 	Change delta;
 	SpawnCommand* cmdPtr = new SpawnCommand(req.name, req.type, req.protect);
 	delta.add(cmdPtr);
 	enqueue(std::pair<int, Change>(req.envId, delta));
 	res.success = true;
-	return true;
+	return;
 }
 
-bool ConfigurationManager::destroyCallback(dyknow_manager::Destroy::Request& req, dyknow_manager::Destroy::Response& res) {
+void ConfigurationManager::destroyCallback(const std::shared_ptr<dyknow_manager::Destroy::Request> req, std::shared_ptr<dyknow_manager::Destroy::Response> res) {
 	Change delta;
 	DestroyCommand* cmdPtr = new DestroyCommand(req.name);
 	delta.add(cmdPtr);
 	enqueue(std::pair<int, Change>(req.envId, delta));
 	res.success = true;
-	return true;
+	return;
 }
 
-bool ConfigurationManager::configureCallback(dyknow_manager::Configure::Request& req, dyknow_manager::Configure::Response& res) {
+void ConfigurationManager::configureCallback(const std::shared_ptr<dyknow_manager::Configure::Request> req, std::shared_ptr<dyknow_manager::Configure::Response> res) {
 	Change delta;
 
 	if(req.config.in_channels.size() != req.config.in_names.size() || req.config.out_channels.size() != req.config.out_names.size()) {
 		res.success = false;
-		return true;
+		return;
 	}
 
 	std::map<std::string, std::string> in;
@@ -132,10 +132,10 @@ bool ConfigurationManager::configureCallback(dyknow_manager::Configure::Request&
 	delta.add(cmdPtr);
 	enqueue(std::pair<int, Change>(req.envId, delta));
 	res.success = true;
-	return true;
+	return;
 }
 
-bool ConfigurationManager::addTargetCallback(dyknow_manager::AddTarget::Request& req, dyknow_manager::AddTarget::Response& res) {
+void ConfigurationManager::addTargetCallback(const std::shared_ptr<dyknow_manager::AddTarget::Request> req, std::shared_ptr<dyknow_manager::AddTarget::Response> res) {
 	Change delta;
 	for (auto& target : req.targets) {
 		TargetSpec spec = TargetFactory::create(target);
@@ -145,10 +145,10 @@ bool ConfigurationManager::addTargetCallback(dyknow_manager::AddTarget::Request&
 
 	enqueue(std::pair<int, Change>(req.envId, delta));
 	res.success = true;
-	return true;
+	return;
 }
 
-bool ConfigurationManager::removeTargetCallback(dyknow_manager::RemoveTarget::Request& req, dyknow_manager::RemoveTarget::Response& res) {
+void ConfigurationManager::removeTargetCallback(const std::shared_ptr<dyknow_manager::RemoveTarget::Request> req, std::shared_ptr<dyknow_manager::RemoveTarget::Response> res) {
 	Change delta;
 	for (auto& label : req.targets) {
 		RemoveTargetCommand* cmdPtr = new RemoveTargetCommand(label);
@@ -157,20 +157,20 @@ bool ConfigurationManager::removeTargetCallback(dyknow_manager::RemoveTarget::Re
 
 	enqueue(std::pair<int, Change>(req.envId, delta));
 	res.success = true;
-	return true;
+	return;
 }
 
-bool ConfigurationManager::refreshCallback(dyknow_manager::Refresh::Request& req, dyknow_manager::Refresh::Response& res) {
+void ConfigurationManager::refreshCallback(const std::shared_ptr<dyknow_manager::Refresh::Request> req, std::shared_ptr<dyknow_manager::Refresh::Response> res) {
 	ROS_INFO("Scheduling refresh");
 	Change delta;
 	RefreshCommand* cmdPtr = new RefreshCommand();
 	delta.add(cmdPtr);
 	enqueue(std::pair<int, Change>(req.envId, delta));
 	res.success = true;
-	return true;
+	return;
 }
 
-bool ConfigurationManager::repairCallback(dyknow_manager::Repair::Request& req, dyknow_manager::Repair::Response& res) {
+void ConfigurationManager::repairCallback(const std::shared_ptr<dyknow_manager::Repair::Request> req, std::shared_ptr<dyknow_manager::Repair::Response> res) {
 	std::lock_guard<std::mutex> guard(envAccess);
 
 	auto iter = std::find_if(environments.begin(), environments.end(), [&req](const Environment& env) {
@@ -180,15 +180,15 @@ bool ConfigurationManager::repairCallback(dyknow_manager::Repair::Request& req,
 	if(iter != environments.end()) {
 		iter->repair = true;
 		res.success = true;
-		return true;
+		return;
 	}
 
 	ROS_WARN("Unable to find environment: %i", req.envId);
 	res.success = false;
-	return false;
+	return;
 }
 
-bool ConfigurationManager::getValidInputsCallback(dyknow_manager::GetValidInputs::Request& req, dyknow_manager::GetValidInputs::Response& res) {
+void ConfigurationManager::getValidInputsCallback(const std::shared_ptr<dyknow_manager::GetValidInputs::Request> req, std::shared_ptr<dyknow_manager::GetValidInputs::Response> res) {
 	std::lock_guard<std::mutex> guard(envAccess);
 
 	auto iter = std::find_if(environments.begin(), environments.end(), [&req](const Environment& env) {
@@ -207,7 +207,7 @@ bool ConfigurationManager::getValidInputsCallback(dyknow_manager::GetValidInputs
 		if(id < 0) {
 			ROS_WARN("Unable to find transformation with label %s; skipping", req.transformation.c_str());
 			res.success = false;
-			return true;
+			return;
 		}
 
 		for (auto& port : iter->getValidInputs(Port(id, req.port-1))) {
@@ -243,14 +243,14 @@ bool ConfigurationManager::getValidInputsCallback(dyknow_manager::GetValidInputs
 		}
 
 		res.success = true;
-		return true;
+		return;
 	}
 
 	ROS_WARN("Unable to find environment: %i", req.env);
-	return true;
+	return;
 }
 
-bool ConfigurationManager::expPopulateCallback(dyknow_manager::ExpPopulate::Request& req, dyknow_manager::ExpPopulate::Response& res) {
+void ConfigurationManager::expPopulateCallback(const std::shared_ptr<dyknow_manager::ExpPopulate::Request> req, std::shared_ptr<dyknow_manager::ExpPopulate::Response> res) {
 	std::lock_guard<std::mutex> guard(envAccess);
 
 	auto iter = std::find_if(environments.begin(), environments.end(), [&req](const Environment& env) {
@@ -260,11 +260,11 @@ bool ConfigurationManager::expPopulateCallback(dyknow_manager::ExpPopulate::Requ
 	if(iter != environments.end()) {
 		iter->expPopulate(req.seed, req.numTransform, req.density, req.lambda, req.numTarget);
 		res.success = true;
-		return true;
+		return;
 	}
 
 	ROS_WARN("Unable to find environment: %i", req.env);
-	return true;
+	return;
 }
 
 void ConfigurationManager::spin() {
@@ -280,7 +280,7 @@ void ConfigurationManager::spin() {
 	}
 }
 
-bool ConfigurationManager::apply(Change delta, int environment) {
+void ConfigurationManager::apply(Change delta, int environment) {
 	std::lock_guard<std::mutex> guard(envAccess);
 
 	auto iter = std::find_if(environments.begin(), environments.end(), [environment](const Environment& env) {
@@ -292,10 +292,10 @@ bool ConfigurationManager::apply(Change delta, int environment) {
 	}
 
 	ROS_WARN("Unable to find environment: %i", environment);
-	return false;
+	return;
 }
 
-bool ConfigurationManager::empty() {
+void ConfigurationManager::empty() {
 	std::lock_guard<std::mutex> guard(deltaAccess);
 	return changeQueue.empty();
 }
diff --git a/src/dyknow_manager/src/configuration_node.cpp b/src/dyknow_manager/src/configuration_node.cpp
new file mode 100644
index 0000000..9d674c1
--- /dev/null
+++ b/src/dyknow_manager/src/configuration_node.cpp
@@ -0,0 +1,57 @@
+#include "configuration_node.hpp"
+
+namespace dyknow {
+
+ConfigurationNode::ConfigurationNode() {
+	this->parentPtr = NULL;
+	this->type = TransformationSpec();
+	this->arg = -1;
+	this->fwdCost = 0;
+	this->outPort = 0;
+	this->cost = MAX_COST + 1;
+	this->future = false;
+}
+
+ConfigurationNode::ConfigurationNode(ConfigurationNode* parentPtr, TransformationSpec type, int arg, int outPort, bool future) {
+	this->parentPtr = parentPtr;
+	this->type = type;
+	this->arg = arg;
+	this->outPort = outPort;
+	this->fwdCost = parentPtr != NULL ? parentPtr->fwdCost + type.cost : 0;
+	this->cost = MAX_COST + 1;
+	this->future = future;
+}
+
+ConfigurationNode::~ConfigurationNode() {
+	std::pair<int, std::vector<ConfigurationNode*> > pair;
+	BOOST_FOREACH(pair, optionMap) {
+		ConfigurationNode* node;
+		BOOST_FOREACH(node, pair.second) {
+			delete node;
+		}
+	}
+}
+
+void ConfigurationNode::addOption(ConfigurationNode* option, int port) {
+	if(optionMap.find(port) == optionMap.end()) {
+		optionMap[port] = std::vector<ConfigurationNode*>();
+	}
+
+	optionMap[port].push_back(option);
+}
+
+double ConfigurationNode::getBestCostChain() {
+	if(parentPtr == NULL) {
+		return this->cost;
+	}
+
+	double bestUp = parentPtr->getBestCostChain();
+	if(bestUp < this->cost) {
+		return bestUp;
+	}
+	else {
+		return this->cost;
+	}
+}
+
+} //namespace
diff --git a/src/dyknow_manager/src/configuration_planner.cpp b/src/dyknow_manager/src/configuration_planner.cpp
new file mode 100644
index 0000000..1f650ef
--- /dev/null
+++ b/src/dyknow_manager/src/configuration_planner.cpp
@@ -0,0 +1,551 @@
+#include "configuration_planner.hpp"
+
+namespace dyknow {
+
+Change ConfigurationPlanner::optimise(Environment* envPtr, unsigned int verbosity) {
+	return optimise(envPtr->getTargets(), envPtr->getComputationGraph(), envPtr->getMatrix(), envPtr->getProtections(), verbosity);
+}
+
+Change ConfigurationPlanner::optimise(std::vector<TargetSpec> targets, ComputationGraph graph, FeasibilityMatrix matrix,
+		std::vector<std::string> protections, unsigned int verbosity) {
+	dyknow::Monitor startMon;
+	startTime = nh->now();
+	
+	
+	expansionCount = 0;
+	this->protections = protections;
+
+	ConfigurationNode* root = new ConfigurationNode();
+	root->type.label = "root";
+	root->cost = targets.size() > 0 ? ConfigurationNode::MAX_COST+1 : 0;
+	expansionCount++;
+
+	std::vector<TransformationSpec> targetTransforms;
+	std::vector<int> targetPorts;
+	TargetSpec target;
+	BOOST_FOREACH(target, targets) {
+		TransformationSpec spec;
+		BOOST_FOREACH(spec, graph.transformations) {
+			if(target.id == spec.id) {
+				targetTransforms.push_back(spec);
+				targetPorts.push_back(target.port);
+				futurePublisherMap[concat(spec.label, "/"+to_string(target.port+1))] = target.topic;
+				ROS_ERROR("Setting target: %s", concat(spec.label, "/"+(target.port+1)).c_str());
+				break;
+			}
+		}
+	}
+
+	for(int i = 0; i < targetTransforms.size(); i++) {
+		root->type.inPorts.push_back("_" + to_string(i));
+
+		// Check for existing CUs that fit the target
+		std::vector<Port> targetCandidates;
+		targetCandidates.push_back(Port(targetTransforms[i].id, targetPorts[i]));
+		bool update = false;
+		expandCompUnit(root, graph, matrix, queue, i, std::vector<TransformationSpec>(), targetCandidates, update);
+
+		// Check for new options for the target
+		expandTransformation(root, matrix, queue, i, std::vector<TransformationSpec>(), targetCandidates, update);
+
+		if(verbosity >= 9) ROS_INFO("Added target %s/%i (arity: %lu)", targetTransforms[i].label.c_str(), targetPorts[i]+1, targetTransforms[i].inPorts.size());
+	}
+
+	int depth = 1;
+	int range = queue.size();
+	if(verbosity >= 3) ROS_INFO("Entering depth: %i", depth);
+	while(expansionCount < MAX_EXPANSIONS && (ros::Time::now() - startTime).toSec() < MAX_TIME_SEC && !queue.empty()) {
+		if(range == 0) {
+			depthExpansionTime.push_back(ros::Time::now()-startTime);
+			depthExpansionIndex.push_back(expansionCount-1);
+			depth++;
+			range = queue.size();
+			if(verbosity >= 3) ROS_INFO("Entering depth %i", depth);
+		}
+
+		ConfigurationNode* node = queue.front();
+		queue.pop();
+		range--;
+
+		std::queue<ConfigurationNode*> expansion = expand(node, graph, matrix, verbosity);
+		if(verbosity >= 9) ROS_INFO("Adding %lu elements to the queue", expansion.size());
+		while(!expansion.empty()) {
+			queue.push(expansion.front());
+			expansion.pop();
+		}
+	}
+
+	// Performance reporting
+	if(verbosity >= 2) ROS_INFO("Done (%i expansions at %f sec)", expansionCount, (ros::Time::now()-startTime).toSec());
+	if(verbosity >= 2) {
+		if(root->cost <= ConfigurationNode::MAX_COST) {
+			ROS_INFO("Found total solution: %f", root->cost);
+			ROS_INFO("---Score change---");
+			for(int index = 0 ; index < costIndex.size(); index++) {
+				ROS_INFO("%i:\t%f\t [%f sec]", costIndex[index], costChange[index], costTime[index].toSec());
+			}
+			ROS_INFO("------------------");
+		}
+		else {
+			ROS_INFO("No total solution found");
+		}
+
+		ROS_INFO("------Target scores------");
+		std::pair<int, ConfigurationNode*> bestElem;
+		BOOST_FOREACH(bestElem, root->bestMap) {
+			ROS_INFO("%i:\t%f",  bestElem.first+1, bestElem.second->cost);
+		}
+		ROS_INFO("----------------------------");
+
+		ROS_INFO("------Expansion depths------");
+		for(int index = 0 ; index < depthExpansionIndex.size(); index++) {
+			ROS_INFO("%i:\t%i\t [%f sec]",  index, depthExpansionIndex[index], depthExpansionTime[index].toSec());
+		}
+		ROS_INFO("----------------------------");
+	}
+
+	root->print();
+
+	// Post-processing to remove duplicate subtrees
+	ros::Time postProcessStartTime = ros::Time::now();
+	generateIndex(root, verbosity);
+	ros::Time postProcessEndTime = ros::Time::now();
+
+	root->print();
+
+	// Convert resulting tree into delta
+	ros::Time changeGenStartTime = ros::Time::now();
+	Change delta;
+	std::pair<int, ConfigurationNode*> bestElem;
+	BOOST_FOREACH(bestElem, root->bestMap) {
+		generateChange(delta, bestElem.second, createUniqueName("stream"), graph, verbosity);
+	}
+	ros::Time changeGenEndTime = ros::Time::now();
+
+	// Eliminate CUs if they are unused and unprotected
+	ros::Time gcStartTime = ros::Time::now();
+//	garbageCollect(delta, graph, verbosity);
+	if(verbosity >= 2) ROS_INFO("Constructed delta of size %i", delta.size());
+	ros::Time gcEndTime = ros::Time::now();
+
+	if(verbosity >= 2) {
+		ROS_INFO("---Method performance---");
+		ros::Duration expandTime;
+		for(int index = 0; index < expandTimes.size(); index++ ) {
+			expandTime += expandTimes[index];
+		}
+		ROS_INFO("Expansion:\t\t%f sec", expandTimes.size() > 0 ? expandTime.toSec() / (double)expandTimes.size() : 0.0);
+
+		ros::Duration expandCUTime;
+		for(int index = 0; index < expandCUTimes.size(); index++ ) {
+			expandCUTime += expandCUTimes[index];
+		}
+		ROS_INFO("\tCU expansion:\t%f sec", expandCUTimes.size() > 0 ? expandCUTime.toSec() / (double)expandCUTimes.size() : 0.0);
+
+		ros::Duration expandTFTime;
+		for(int index = 0; index < expandTFTimes.size(); index++ ) {
+			expandTFTime += expandTFTimes[index];
+		}
+		ROS_INFO("\tTF expansion:\t%f sec", expandTFTimes.size() > 0 ? expandTFTime.toSec() / (double)expandTFTimes.size() : 0.0);
+
+		ros::Duration updateTime;
+		for(int index = 0; index < updateTimes.size(); index++ ) {
+			updateTime += updateTimes[index];
+		}
+		ROS_INFO("\tUpdate:\t\t%f sec", updateTimes.size() > 0 ? updateTime.toSec() / (double)updateTimes.size() : 0.0);
+
+		ros::Duration postProcessTime = postProcessEndTime - postProcessStartTime;
+		ROS_INFO("Post-process:\t\t%f sec", postProcessTime.toSec());
+
+		ros::Duration generateChangeTime = changeGenEndTime - changeGenStartTime;
+		ROS_INFO("Change generation:\t%f sec", generateChangeTime.toSec());
+
+		ros::Duration garbageCollectTime = gcEndTime - gcStartTime;
+		ROS_INFO("Garbage collection:\t%f sec", garbageCollectTime.toSec());
+
+		ROS_INFO("----------------------------");
+	}
+
+	// Clean and return result
+	delete root;
+	reset();
+
+	dyknow::Monitor endMon;
+	(endMon-startMon).print();
+	return delta;
+}
+
+std::queue<ConfigurationNode*> ConfigurationPlanner::expand(ConfigurationNode* nodePtr, ComputationGraph& graph, FeasibilityMatrix& matrix, unsigned int verbosity) {
+	ros::Time expandStart = ros::Time::now();
+	std::queue<ConfigurationNode*> result;
+	int arity = nodePtr->type.inPorts.size();
+	bool update = false;
+
+	// Fetch transformation path for exclusion list
+	std::vector<TransformationSpec> ancestors;
+	getAncestors(nodePtr, ancestors);
+
+	// Iterate over arguments
+	for(int i = 0 ; i < arity ; i++) {
+		std::vector<Port> candidates = matrix.getValid(Port(nodePtr->type.id, i));
+		ros::Time expandCUStart = ros::Time::now();
+		expandCompUnit(nodePtr, graph, matrix, result, i, ancestors, candidates, update, verbosity);
+		ros::Time expandTFStart = ros::Time::now();
+		expandTransformation(nodePtr, matrix, result, i, ancestors, candidates, update, verbosity);
+		ros::Time expandFinish = ros::Time::now();
+
+		expandCUTimes.push_back(expandTFStart - expandCUStart);
+		expandTFTimes.push_back(expandFinish - expandTFStart);
+	}
+
+	// Only update when needed
+	if(update) {
+		ros::Time updateStart = ros::Time::now();
+		this->update(nodePtr, matrix);
+		ros::Time updateFinish = ros::Time::now();
+
+		updateTimes.push_back(updateFinish - updateStart);
+	}
+
+	if(verbosity >= 9) ROS_INFO("Expansion count: %i", expansionCount);
+	ros::Time expandFinish = ros::Time::now();
+	expandTimes.push_back(expandFinish - expandStart); //TODO: Move timers
+	return result;
+}
+
+void ConfigurationPlanner::expandCompUnit(ConfigurationNode* nodePtr, ComputationGraph& graph, FeasibilityMatrix& matrix, std::queue<ConfigurationNode*>& queue,
+		int index, std::vector<TransformationSpec> ancestors, std::vector<Port> candidates, bool& update, unsigned int verbosity) {
+
+	// Consider using computation units
+	double bestCost = nodePtr->hasBest(index) ? nodePtr->bestMap[index]->cost : ConfigurationNode::MAX_COST + 1;
+
+	Port candidate;
+	BOOST_FOREACH(candidate, candidates) {
+		TransformationSpec candidateSpec = matrix.decode(candidate);
+		if(candidateSpec.id < 0) { continue; }
+//		if(verbosity >= 9) ROS_INFO("Considering (%s, /%i | arity: %lu) as input for (%s, _%i | arity: %lu)", candidateSpec.label.c_str(), candidate.second, candidateSpec.inPorts.size(), nodePtr->type.label.c_str(), index, nodePtr->type.inPorts.size());
+
+		ComputationUnit compUnit;
+		BOOST_FOREACH(compUnit, graph.computationUnits) {
+
+			if(candidateSpec.label == compUnit.type && find(ancestors.begin(), ancestors.end(), candidateSpec) == ancestors.end()) {
+				if(verbosity >= 9) { ROS_INFO("Found matching computation unit %s for transformation %s with port /%i", compUnit.name.c_str(), candidateSpec.label.c_str(), candidate.second+1); }
+
+
+				TransformationSpec spec;
+				spec.label = compUnit.name;
+				spec.type = COMPUNIT;
+				spec.source = candidateSpec.label;
+				spec.inPorts = candidateSpec.inPorts;
+				spec.cost = 0;
+				spec.id = candidateSpec.id;
+				spec.outPorts = candidateSpec.outPorts;
+
+				if(bestCost > ConfigurationNode::MAX_COST || spec.cost + nodePtr->fwdCost < nodePtr->getBestCostChain()) {
+					ConfigurationNode* option = new ConfigurationNode(nodePtr, spec, index, candidate.second);
+					logPerformance(verbosity);
+					expansionCount++;
+					int optionArity = spec.inPorts.size();
+					nodePtr->addOption(option, index);
+					if(verbosity >= 9) ROS_INFO("Using (%s, /%i | arity: %lu) as input for (%s, _%i | arity: %lu)", candidateSpec.label.c_str(), candidate.second, candidateSpec.inPorts.size(), nodePtr->type.label.c_str(), index, nodePtr->type.inPorts.size());
+					if(optionArity > 0) {
+						queue.push(option);
+					}
+					else {
+						option->cost = 0;
+						update = true;
+						//this->update(nodePtr, matrix, verbosity); // Optional for easier debugging
+					}
+				}
+
+			}
+
+		}
+	}
+}
+
+void ConfigurationPlanner::expandTransformation(ConfigurationNode* nodePtr, FeasibilityMatrix& matrix, std::queue<ConfigurationNode*>& queue, int index,
+		std::vector<TransformationSpec> ancestors, std::vector<Port> candidates, bool& update, unsigned int verbosity) {
+
+	// Consider using a new instance
+	double bestCost = nodePtr->hasBest(index) ? nodePtr->bestMap[index]->cost : ConfigurationNode::MAX_COST + 1;
+
+	Port candidate;
+	BOOST_FOREACH(candidate, candidates) {
+		TransformationSpec candidateSpec = matrix.decode(candidate);
+//		if(verbosity >= 9) ROS_INFO("Considering (%s, /%i | arity: %lu) as input for (%s, _%i | arity: %lu)", candidateSpec.label.c_str(), candidate.second, candidateSpec.inPorts.size(), nodePtr->type.label.c_str(), index, nodePtr->type.inPorts.size());
+		if(find(ancestors.begin(), ancestors.end(), candidateSpec) != ancestors.end()) {
+			// Not allowed to reuse this transformation along this path
+			if(verbosity >= 3) ROS_INFO("May not reuse: %s (skipping)", candidateSpec.label.c_str());
+			continue;
+		}
+
+
+		// Consider children if there is no solution, or if the new potential solution does not exceed the best one
+		if(bestCost > ConfigurationNode::MAX_COST || candidateSpec.cost + nodePtr->fwdCost < nodePtr->getBestCostChain()) {
+			ConfigurationNode* option = new ConfigurationNode(nodePtr, candidateSpec, index, candidate.second);
+			logPerformance(verbosity);
+			expansionCount++;
+			int optionArity = candidateSpec.inPorts.size();
+			nodePtr->addOption(option, index);
+			if(verbosity >= 9) ROS_INFO("Using (%s, /%i | arity: %lu) as input for (%s, _%i | arity: %lu)", candidateSpec.label.c_str(), candidate.second, candidateSpec.inPorts.size(), nodePtr->type.label.c_str(), index, nodePtr->type.inPorts.size());
+			if(optionArity > 0) {
+				queue.push(option);
+			}
+			else {
+				option->cost = candidateSpec.cost;
+				update = true;
+				//this->update(nodePtr, matrix, verbosity); // Optional for easier debugging
+			}
+		}
+	}
+}
+
+void ConfigurationPlanner::update(ConfigurationNode* nodePtr, FeasibilityMatrix& matrix, unsigned int verbosity) {
+	if(nodePtr == NULL) { return; }
+
+	int arity = nodePtr->type.inPorts.size();
+	bool cascade = true;
+	double cost = nodePtr->type.cost;
+	for(int i = 0 ; i < arity ; i++) {
+		std::vector<ConfigurationNode*> options;
+		if(nodePtr->hasOptions(i)) { options = nodePtr->optionMap[i]; }
+		ConfigurationNode* option;
+		BOOST_FOREACH(option, options) {
+			double newCost = option->cost;
+			double best = nodePtr->hasBest(i) ? nodePtr->bestMap[i]->cost : ConfigurationNode::MAX_COST+1;
+			if(newCost < best) {
+				nodePtr->bestMap[i] = option;
+				if(verbosity >= 9) ROS_INFO("New best (%s): %f < %f", option->type.label.c_str(), newCost, best);
+			}
+			else {
+				if(verbosity >= 9) ROS_INFO("Old best (%s): %f is not better than %f", option->type.label.c_str(), newCost, best);
+			}
+		}
+
+		cascade = cascade && nodePtr->hasBest(i);
+		if(nodePtr->hasBest(i) && nodePtr->bestMap[i]->cost <= ConfigurationNode::MAX_COST) {
+			cost += nodePtr->bestMap[i]->cost;
+		}
+		else {
+			cost = ConfigurationNode::MAX_COST+1;
+		}
+	}
+	if(verbosity >= 9) ROS_INFO("New cost (%s): %f", nodePtr->type.label.c_str(), cost);
+
+	if(cascade) {
+		nodePtr->cost = cost;
+		this->update(nodePtr->parentPtr, matrix, verbosity);
+	}
+
+	if(nodePtr->parentPtr == NULL && prevBest != nodePtr->cost) {
+		if(verbosity >= 9) ROS_INFO("***** Found solution with cost %f *****", nodePtr->cost);
+		prevBest = nodePtr->cost;
+		costChange.push_back(nodePtr->cost);
+		costIndex.push_back(expansionCount);
+		costTime.push_back(ros::Time::now() - startTime);
+	}
+}
+
+void ConfigurationPlanner::generateChange(Change& change, ConfigurationNode* nodePtr, std::string outStream, ComputationGraph& graph, unsigned int verbosity) {
+
+	// Futures are placeholders that cannot be configured
+	if(nodePtr->isFuture()) {
+		if(verbosity >= 9) ROS_INFO("Skipping future %s", nodePtr->type.source.c_str() );
+		return;
+	}
+
+	// Create transformation instance
+	std::string name = nodePtr->type.label;
+	bool futureNode = false;
+	if(nodePtr->type.type == NODELET) {
+		if(futureNodeMap.count(nodePtr->type.label) > 0) {
+			// Already prepared for spawning; set to future to skip subscription configuration
+			name = futureNodeMap[nodePtr->type.label];
+			futureNode = true;
+		}
+		else {
+			// Prepare to spawn a new CU of this type
+			name = createUniqueName(nodePtr->type.label);
+			SpawnCommand* spawnCmdPtr = new SpawnCommand(name, nodePtr->type.label, false);
+			change.add(spawnCmdPtr);
+			futureNodeMap[nodePtr->type.label] = name;
+		}
+	}
+
+	// Set outgoing stream
+	std::map<std::string, std::string> out;
+	std::string outPort = "/" + to_string(nodePtr->outPort+1);
+
+//	ROS_ERROR("Processing %s/%s (%s)", nodePtr->type.label.c_str(), outPort.c_str(), nodePtr->type.source.c_str());
+
+	if(nodePtr->type.type == NODELET && futurePublisherMap.count(concat(nodePtr->type.label, outPort)) > 0) {
+		out[outPort] = futurePublisherMap[concat(nodePtr->type.label, outPort)];
+	}
+	else if(nodePtr->type.type == COMPUNIT && futurePublisherMap.count(concat(nodePtr->type.source, outPort)) > 0) {
+		out[outPort] = futurePublisherMap[concat(nodePtr->type.source, outPort)];
+	}
+	else {
+		std::string key = nodePtr->type.type == NODELET ? nodePtr->type.label : nodePtr->type.source;
+		futurePublisherMap[concat(key, outPort)] = outStream;
+		if(verbosity >= 9) ROS_INFO("Setting future: %s => %s", concat(key, outPort).c_str(), outStream.c_str());
+		out[outPort] = outStream;
+	}
+
+	// Set incoming streams if this has not been done already
+	// IMPORTANT: Note that the output ports refer to those of the best elements, not of the current node!
+	std::map<std::string, std::string> in;
+	if(!futureNode) {
+		std::pair<int, ConfigurationNode*> bestElem;
+		BOOST_FOREACH(bestElem, nodePtr->bestMap) {
+			std::string inPort = "_" + to_string(bestElem.first+1);
+			if(bestElem.second->type.type == NODELET) {
+				// Create a new stream unless a target overrides it
+				if(futurePublisherMap.count(concat(bestElem.second->type.label, "/" + to_string(bestElem.second->outPort+1))) > 0) {
+					in[inPort] = futurePublisherMap[concat(bestElem.second->type.label, "/" + to_string(bestElem.second->outPort+1))];
+//					ROS_ERROR("[%s] Reusing TF %s: %s", nodePtr->type.label.c_str(), concat(bestElem.second->type.label, "/" + to_string(bestElem.second->outPort+1)).c_str(), in[inPort].c_str());
+				}
+				else {
+					in[inPort] = createUniqueName("stream");
+				}
+			}
+			else if(bestElem.second->type.type == COMPUNIT) {
+				// Reuse existing stream if one exists
+				std::string streamName = createUniqueName("stream");
+
+				if(bestElem.second->isFuture()) {
+					// Acquire stream name from future map if one exists (may not be the case for all ports)
+//					if(futurePublisherMap.count(concat(bestElem.second->type.source, outPort)) > 0) {
+					streamName = futurePublisherMap[concat(bestElem.second->type.source, "/" + to_string(bestElem.second->outPort+1))];
+//					ROS_ERROR("[%s] Reusing CU %s: %s", nodePtr->type.source.c_str(), concat(bestElem.second->type.source, "/" + to_string(bestElem.second->outPort+1)).c_str(), streamName.c_str());
+//					}
+//					else {
+//						futurePublisherMap[concat(bestElem.second->type.source, outPort)] = streamName;
+//						if(verbosity >= 9) ROS_INFO("Setting future: %s => %s", concat(bestElem.second->type.source, outPort).c_str(), streamName.c_str());
+//					}
+					if(verbosity >= 3) ROS_INFO("Using future stream %s for port %s of CU %s", streamName.c_str(), inPort.c_str(), name.c_str());
+				}
+				else {
+					// Acquire stream name from known graph
+					std::string cuName = bestElem.second->type.label;
+					std::pair<std::string, std::string> pair;
+					BOOST_FOREACH(pair, graph.generatorMap[cuName]) {
+						std::string bestOutPort = "/" + to_string(bestElem.second->outPort+1);
+						if(pair.first.compare(bestOutPort) == 0) {
+							streamName = pair.second;
+							break;
+						}
+					}
+				}
+
+				// Connect stream to input
+				in[inPort] = streamName;
+			}
+
+			// Recurse
+			generateChange(change, bestElem.second, in[inPort], graph, verbosity);
+		}
+	}
+
+	ConfigureCommand* configureCmdPtr = new ConfigureCommand(name, in, out);
+	change.add(configureCmdPtr);
+
+	if(verbosity >= 3) {
+		std::pair<std::string, std::string> pair;
+		BOOST_FOREACH(pair, in) {
+			ROS_INFO("Setting %s incoming:\t%s (%s)", name.c_str(), pair.second.c_str(), pair.first.c_str());
+		}
+		BOOST_FOREACH(pair, out) {
+			ROS_INFO("Setting %s outgoing:\t%s (%s)", name.c_str(), pair.second.c_str(), pair.first.c_str());
+		}
+	}
+
+	if(nodePtr->type.type == COMPUNIT) {
+		if(verbosity >= 3) ROS_INFO("Protecting CU %s", name.c_str());
+		protections.push_back(name);
+	}
+}
+
+void ConfigurationPlanner::garbageCollect(Change& change, ComputationGraph& graph, unsigned int verbosity) {
+	for(int i = 0; i < graph.computationUnits.size(); i++) {
+		if(find(protections.begin(), protections.end(), graph.computationUnits[i].name) == protections.end()) {
+			// Allowed to remove this CU
+			if(verbosity >= 3) ROS_INFO("Marking CU %s for unloading", graph.computationUnits[i].name.c_str());
+
+			DestroyCommand* destroyCmdPtr = new DestroyCommand(graph.computationUnits[i].name);
+			change.add(destroyCmdPtr);
+		}
+	}
+}
+
+void ConfigurationPlanner::generateIndex(ConfigurationNode* nodePtr, unsigned int verbosity) {
+
+	std::pair<int, ConfigurationNode*> bestCandidate;
+	BOOST_FOREACH(bestCandidate, nodePtr->bestMap) {
+		if(nodePtr->isRoot() && bestCandidate.second->cost > ConfigurationNode::MAX_COST) {
+			// Exclude invalid subtrees
+			ROS_WARN("No solution for target %s at root port %i; skipping", bestCandidate.second->type.label.c_str(), bestCandidate.first);
+			continue;
+		}
+
+		int outPort = bestCandidate.second->outPort;
+		std::string portLabel = concat(bestCandidate.second->type.label, "/" + to_string(outPort+1));
+		std::string nodeLabel = bestCandidate.second->type.label;
+
+
+		if(portIndex.count(portLabel) == 0) {
+			// Index this key to guard for duplication
+			portIndex[portLabel] = bestCandidate.second;
+			if(verbosity >=9) ROS_INFO("Guarding %s for duplication", portLabel.c_str());
+
+			// Recursively apply subtree removal
+			generateIndex(bestCandidate.second, verbosity);
+		}
+		else {
+
+			// Found a duplicate subtree; replace with future CU
+			if(verbosity >= 3) ROS_INFO("Swapping duplicate tree with root %s for future", portLabel.c_str());
+//			int outPort = bestCandidate.second->outPort;
+			TransformationSpec spec = bestCandidate.second->type;
+			spec.type = COMPUNIT;
+			if(bestCandidate.second->type.type == NODELET) {
+				spec.source = bestCandidate.second->type.label;
+				spec.label = "FUTURE";
+			}
+
+			nodePtr->bestMap[bestCandidate.first] = new ConfigurationNode(nodePtr, spec, bestCandidate.first, outPort, true);
+		}
+	}
+}
+
+void ConfigurationPlanner::getAncestors(ConfigurationNode* nodePtr, std::vector<TransformationSpec>& ancestors) {
+	if(!nodePtr->type.canRecurse && nodePtr->type.type == NODELET) {
+		ancestors.push_back(nodePtr->type);
+	}
+
+	if(nodePtr->parentPtr != NULL) {
+		getAncestors(nodePtr->parentPtr, ancestors);
+	}
+}
+
+std::string ConfigurationPlanner::createUniqueName(std::string base) {
+	//TODO: Synchronised access
+
+	// Empty base strings are not allowed
+	if(base == "") {
+		ROS_WARN("Empty base strings are not allowed for unique names");
+		base = "reserved";
+	}
+
+	// Add new label if it does not exist yet
+	if (uniqueNameMap.find(base) == uniqueNameMap.end()) {
+		uniqueNameMap[base] = 0;
+	}
+
+	// Append label with dyknow_ and prepend it with the unique identifier
+	std::string suffix = boost::lexical_cast<std::string>((int)uniqueNameMap[base]);
+	uniqueNameMap[base]++;
+	return ("/dyknow/" + base + "_" + suffix);
+}
+
+} //namespace
diff --git a/src/dyknow_manager/src/environment.cpp b/src/dyknow_manager/src/environment.cpp
index b5d1c94..6207730 100644
--- a/src/dyknow_manager/src/environment.cpp
+++ b/src/dyknow_manager/src/environment.cpp
@@ -41,7 +41,7 @@ bool Environment::spawn(std::string name, std::string type, bool protect) {
 			args.push_back(param.value); //FIXME: The interface currently ignores parameter names and types
 		}
 
-		if (CompUnitHelper::spawn(nh, nodeletManagerName, name, iter->source, args)) {
+		if (CompUnitHelper::spawn(h, nodeletManagerName, name, iter->source, args)) {
 			ComputationUnit cu(name, type);
 			computationUnits.push_back(cu);
 			subscriptionMap.emplace(name, Subscription());
diff --git a/src/dyknow_manager/src/feasibility_matrix.cpp b/src/dyknow_manager/src/feasibility_matrix.cpp
new file mode 100644
index 0000000..83fec11
--- /dev/null
+++ b/src/dyknow_manager/src/feasibility_matrix.cpp
@@ -0,0 +1,40 @@
+#include "feasibility_matrix.hpp"
+
+namespace dyknow {
+
+FeasibilityMatrix::FeasibilityMatrix(std::vector<TransformationSpec> library, double density) {
+	RandomMatrixGenerator gen(density);
+	this->matrix = gen.generate(library);
+	this->library = library;
+}
+
+std::vector<Port> FeasibilityMatrix::getValid(Port input) {
+	return this->matrix[input];
+}
+
+TransformationSpec FeasibilityMatrix::decode(Port port) {
+	TransformationSpec spec;
+	BOOST_FOREACH(spec, library) {
+		if(spec.id == port.first) {
+			return spec;
+		}
+	}
+
+	ROS_WARN("Unable to decode port (%i, %i)", port.first, port.second);
+	return spec;
+}
+
+void FeasibilityMatrix::invalidate(std::string transformation) {
+
+	for(int i = 0; i < library.size(); i++) {
+		if(library[i].label == transformation) {
+			// Effectively prevent transformation from being used until matrix is recompiled
+			library[i].cost = INT_MAX;
+			return;
+		}
+	}
+
+	ROS_WARN("Unknown transformation %s marked for invalidation; ignoring", transformation.c_str());
+}
+
+} // namespace
diff --git a/src/dyknow_manager/src/manager_componet.cpp b/src/dyknow_manager/src/manager_componet.cpp
index b10cab3..503b6bd 100644
--- a/src/dyknow_manager/src/manager_componet.cpp
+++ b/src/dyknow_manager/src/manager_componet.cpp
@@ -6,7 +6,7 @@ namespace manager {
 	ManagerComponet::ManagerComponet(const rclcpp::NodeOptions & options): 
 		node(std::make_shared<dyknow::Node>("manager_componet", options)) {
 
-			//configManPtr = std::shared_ptr<ConfigurationManager>(new ConfigurationManager(node));
+			std::shared_ptr<dyknow::ConfigurationManager> configManPtr = std::shared_ptr<dyknow::ConfigurationManager>(new dyknow::ConfigurationManager(node));
 	
 		}
 
diff --git a/src/dyknow_manager/srv/ExpPopulate.srv b/src/dyknow_manager/srv/ExpPopulate.srv
index 23867e9..80a0f2b 100644
--- a/src/dyknow_manager/srv/ExpPopulate.srv
+++ b/src/dyknow_manager/srv/ExpPopulate.srv
@@ -1,9 +1,8 @@
-# Experimental service!
 uint32 env
 uint32 seed
-uint32 numTransform=1000
-float32 lambda=2
-uint32 numTarget
-float32 density=0.01
+uint32 numtransform 
+float32 lambda 
+uint32 numtarget
+float32 density
 ---
 bool success
\ No newline at end of file
diff --git a/src/dyknow_nodehandle/src/node.cpp b/src/dyknow_nodehandle/src/node.cpp
index e713e5d..902646e 100644
--- a/src/dyknow_nodehandle/src/node.cpp
+++ b/src/dyknow_nodehandle/src/node.cpp
@@ -1,3 +1,2 @@
 #include <dyknow_nodehandle/node.hpp>
 //There seems to be something strange happening here, for some reason I am unable to define a function in the source.
-// Could have to do with the build process.... 
\ No newline at end of file
-- 
GitLab