From 78d06fab4d3a8bd90bc83964304968cf3988fd56 Mon Sep 17 00:00:00 2001
From: leoja464 <leoja464@student.liu.se>
Date: Tue, 23 Jul 2024 19:38:05 +0200
Subject: [PATCH] started on DyKnow manager implementation

---
 src/dyknow_echo/CMakeLists.txt                |   8 +-
 src/dyknow_echo/include/dyknow_echo/echo.hpp  |   1 -
 .../include/dyknow_echo/echoComposition.hpp   |  28 ++
 .../load_echo_node.launch.cpython-312.pyc     | Bin 1768 -> 1772 bytes
 .../launch/load_echo_node.launch.py           |   2 +-
 src/dyknow_echo/src/echoComposition.cpp       |  36 ++
 src/dyknow_manager/CMakeLists.txt             | 107 ++++--
 .../include/dyknow_manager/change.hpp         | 115 ++++++
 .../dyknow_manager/configuration_manager.hpp  | 119 +++++++
 .../include/dyknow_manager/environment.hpp    | 103 ++++++
 .../dyknow_manager/manager_componet.hpp       |  26 ++
 .../include/dyknow_manager/target_spec.hpp    |  44 +++
 .../dyknow_manager/transformation_spec.hpp    |  75 ++++
 .../dyknow_manager/util/from_string.hpp       |  27 ++
 .../include/dyknow_manager/util/to_string.hpp |  30 ++
 src/dyknow_manager/msg/Connection.msg         |   2 +
 src/dyknow_manager/msg/Parameter.msg          |   3 +
 src/dyknow_manager/msg/Tag.msg                |   2 +
 src/dyknow_manager/msg/Target.msg             |   4 +
 src/dyknow_manager/msg/Transformation.msg     |   9 +
 src/dyknow_manager/msg/Unit.msg               |   5 +
 src/dyknow_manager/package.xml                |   6 +
 src/dyknow_manager/src/change.cpp             |  55 +++
 .../src/configuration_manager.cpp             | 331 ++++++++++++++++++
 src/dyknow_manager/src/environment.cpp        | 160 +++++++++
 src/dyknow_manager/src/manager_componet.cpp   |  15 +
 src/dyknow_manager/src/manager_node.cpp       |  10 -
 src/dyknow_manager/src/target_spec.cpp        |  18 +
 .../src/transformation_spec.cpp               |  39 +++
 src/dyknow_manager/srv/AddTarget.srv          |   4 +
 src/dyknow_manager/srv/AddTransformation.srv  |   4 +
 src/dyknow_manager/srv/Configure.srv          |   5 +
 src/dyknow_manager/srv/CreateStream.srv       |   4 +
 src/dyknow_manager/srv/Destroy.srv            |   4 +
 src/dyknow_manager/srv/ExpPopulate.srv        |   9 +
 src/dyknow_manager/srv/GetConfiguration.srv   |   4 +
 src/dyknow_manager/srv/GetUniqueName.srv      |   4 +
 src/dyknow_manager/srv/GetValidInputs.srv     |   7 +
 src/dyknow_manager/srv/Refresh.srv            |   3 +
 src/dyknow_manager/srv/RemoveTarget.srv       |   4 +
 .../srv/RemoveTransformation.srv              |   4 +
 src/dyknow_manager/srv/Repair.srv             |   3 +
 src/dyknow_manager/srv/RequestUnload.srv      |   3 +
 src/dyknow_manager/srv/Spawn.srv              |   6 +
 src/dyknow_nodehandle/package.xml             |   1 -
 45 files changed, 1411 insertions(+), 38 deletions(-)
 create mode 100644 src/dyknow_echo/include/dyknow_echo/echoComposition.hpp
 create mode 100644 src/dyknow_echo/src/echoComposition.cpp
 create mode 100644 src/dyknow_manager/include/dyknow_manager/change.hpp
 create mode 100644 src/dyknow_manager/include/dyknow_manager/configuration_manager.hpp
 create mode 100644 src/dyknow_manager/include/dyknow_manager/environment.hpp
 create mode 100644 src/dyknow_manager/include/dyknow_manager/manager_componet.hpp
 create mode 100644 src/dyknow_manager/include/dyknow_manager/target_spec.hpp
 create mode 100644 src/dyknow_manager/include/dyknow_manager/transformation_spec.hpp
 create mode 100644 src/dyknow_manager/include/dyknow_manager/util/from_string.hpp
 create mode 100644 src/dyknow_manager/include/dyknow_manager/util/to_string.hpp
 create mode 100644 src/dyknow_manager/msg/Connection.msg
 create mode 100644 src/dyknow_manager/msg/Parameter.msg
 create mode 100644 src/dyknow_manager/msg/Tag.msg
 create mode 100644 src/dyknow_manager/msg/Target.msg
 create mode 100644 src/dyknow_manager/msg/Transformation.msg
 create mode 100644 src/dyknow_manager/msg/Unit.msg
 create mode 100644 src/dyknow_manager/src/change.cpp
 create mode 100644 src/dyknow_manager/src/configuration_manager.cpp
 create mode 100644 src/dyknow_manager/src/environment.cpp
 create mode 100644 src/dyknow_manager/src/manager_componet.cpp
 delete mode 100644 src/dyknow_manager/src/manager_node.cpp
 create mode 100644 src/dyknow_manager/src/target_spec.cpp
 create mode 100644 src/dyknow_manager/src/transformation_spec.cpp
 create mode 100644 src/dyknow_manager/srv/AddTarget.srv
 create mode 100644 src/dyknow_manager/srv/AddTransformation.srv
 create mode 100644 src/dyknow_manager/srv/Configure.srv
 create mode 100644 src/dyknow_manager/srv/CreateStream.srv
 create mode 100644 src/dyknow_manager/srv/Destroy.srv
 create mode 100644 src/dyknow_manager/srv/ExpPopulate.srv
 create mode 100644 src/dyknow_manager/srv/GetConfiguration.srv
 create mode 100644 src/dyknow_manager/srv/GetUniqueName.srv
 create mode 100644 src/dyknow_manager/srv/GetValidInputs.srv
 create mode 100644 src/dyknow_manager/srv/Refresh.srv
 create mode 100644 src/dyknow_manager/srv/RemoveTarget.srv
 create mode 100644 src/dyknow_manager/srv/RemoveTransformation.srv
 create mode 100644 src/dyknow_manager/srv/Repair.srv
 create mode 100644 src/dyknow_manager/srv/RequestUnload.srv
 create mode 100644 src/dyknow_manager/srv/Spawn.srv

diff --git a/src/dyknow_echo/CMakeLists.txt b/src/dyknow_echo/CMakeLists.txt
index 2c649cf..622fc01 100644
--- a/src/dyknow_echo/CMakeLists.txt
+++ b/src/dyknow_echo/CMakeLists.txt
@@ -25,6 +25,7 @@ include_directories(
 # Declare a shared library
 add_library(dyknow_echo_component SHARED
   src/echo.cpp
+  src/echoComposition.cpp
 )
 
 target_include_directories(dyknow_echo_component PUBLIC
@@ -39,8 +40,13 @@ ament_target_dependencies(dyknow_echo_component
   dyknow_analytics
 )
 
+#rclcpp_components_register_node(dyknow_echo_component
+#  PLUGIN "echo::Echo"
+#  EXECUTABLE dyknow_echo
+#)
+
 rclcpp_components_register_node(dyknow_echo_component
-  PLUGIN "echo::Echo"
+  PLUGIN "echo::EchoComp"
   EXECUTABLE dyknow_echo
 )
 
diff --git a/src/dyknow_echo/include/dyknow_echo/echo.hpp b/src/dyknow_echo/include/dyknow_echo/echo.hpp
index 5a2e1e9..96316bd 100644
--- a/src/dyknow_echo/include/dyknow_echo/echo.hpp
+++ b/src/dyknow_echo/include/dyknow_echo/echo.hpp
@@ -6,7 +6,6 @@
 //#include <dyknow_nodehandle/node_handle.hpp>
 #include <dyknow_nodehandle/msg/sample.hpp>
 #include <memory>
-#include <dyknow_nodehandle/test_node.hpp>
 
 namespace echo {
 
diff --git a/src/dyknow_echo/include/dyknow_echo/echoComposition.hpp b/src/dyknow_echo/include/dyknow_echo/echoComposition.hpp
new file mode 100644
index 0000000..2926ddd
--- /dev/null
+++ b/src/dyknow_echo/include/dyknow_echo/echoComposition.hpp
@@ -0,0 +1,28 @@
+#ifndef ECHO_COMP_H_
+#define ECHO_COMP_H_
+
+#include <rclcpp/rclcpp.hpp>
+#include <dyknow_nodehandle/node.hpp>
+//#include <dyknow_nodehandle/node_handle.hpp>
+#include <dyknow_nodehandle/msg/sample.hpp>
+#include <memory>
+
+namespace echo {
+
+class EchoComp {
+	public:
+		EchoComp(const rclcpp::NodeOptions & options);
+	    void callback(std::shared_ptr<dyknow_nodehandle::msg::Sample>& sample);
+
+
+		rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
+  		get_node_base_interface() const;
+
+	private:
+		dyknow::Publisher pub;
+		std::shared_ptr<dyknow::Node> node;
+};
+
+} // namespace dyknow
+
+#endif /* ECHO_COMP_H_ */
diff --git a/src/dyknow_echo/launch/__pycache__/load_echo_node.launch.cpython-312.pyc b/src/dyknow_echo/launch/__pycache__/load_echo_node.launch.cpython-312.pyc
index c79231937ee6a249f9a43a046d983d3d279aad0a..e50be9a0d15e38384721c2a92af7c801effe4f32 100644
GIT binary patch
delta 50
zcmaFC`-YeIG%qg~0|NuY=BRmT!W(&$nfdurlQZ(ItXz{b@}2W@3pQ6X-(_T!-yF%>
G&j<j0I1fPp

delta 46
zcmaFE`+}GEG%qg~0|NuY<BhY^_&4$<GjnsLCTHYZS-B=><ZrHLzRSocy*ZY(pAi60
C2o5d)

diff --git a/src/dyknow_echo/launch/load_echo_node.launch.py b/src/dyknow_echo/launch/load_echo_node.launch.py
index e9609d7..87918f0 100644
--- a/src/dyknow_echo/launch/load_echo_node.launch.py
+++ b/src/dyknow_echo/launch/load_echo_node.launch.py
@@ -25,7 +25,7 @@ def generate_launch_description():
     # Define the composable node
     echo_node = ComposableNode(
         package='dyknow_echo',
-        plugin='echo::Echo',
+        plugin='echo::EchoComp',
         name='echo_node',
         namespace='dyknow_echo',
         parameters=[],
diff --git a/src/dyknow_echo/src/echoComposition.cpp b/src/dyknow_echo/src/echoComposition.cpp
new file mode 100644
index 0000000..4bce894
--- /dev/null
+++ b/src/dyknow_echo/src/echoComposition.cpp
@@ -0,0 +1,36 @@
+#include "dyknow_echo/echoComposition.hpp"
+#include <rclcpp_components/register_node_macro.hpp>
+
+
+namespace echo {
+
+    EchoComp::EchoComp(const rclcpp::NodeOptions & options): node(std::make_shared<dyknow::Node>("echo_node", options)) 
+    {
+        int arity = 10;
+        for(int i = 0; i < arity; i++) {
+            node->create_subscription<dyknow_nodehandle::msg::Sample, EchoComp>(1000, &EchoComp::callback, this);
+        }
+        node->connectInput("_1", "in");
+        pub = node->create_publisher<dyknow_nodehandle::msg::Sample>(1000);
+        node->connectOutput("/1", "out");
+    }
+
+
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr EchoComp::get_node_base_interface() const
+    {
+    return this->node->get_node_base_interface();
+    }
+
+
+    void EchoComp::callback(std::shared_ptr<dyknow_nodehandle::msg::Sample>& sample) {
+        dyknow_nodehandle::msg::Sample copy;
+        copy.fields = sample->fields;
+        copy.header = sample->header;
+        copy.valid_time = sample->valid_time;
+        RCLCPP_INFO(node->get_logger(), "I LIVE, AM I ALIVE, I AM MEEEEE ");
+        pub.publish(copy);
+    }
+
+} // namespace dyknow
+
+RCLCPP_COMPONENTS_REGISTER_NODE(echo::EchoComp)
diff --git a/src/dyknow_manager/CMakeLists.txt b/src/dyknow_manager/CMakeLists.txt
index 42bb029..227391e 100644
--- a/src/dyknow_manager/CMakeLists.txt
+++ b/src/dyknow_manager/CMakeLists.txt
@@ -7,29 +7,88 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-
-add_executable(manager_node src/manager_node.cpp)
-target_include_directories(manager_node PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
-target_compile_features(manager_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
-
-install(TARGETS manager_node
-  DESTINATION lib/${PROJECT_NAME})
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
+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)
+
+# Add message and service files
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Tag.msg"
+  "msg/Parameter.msg"
+  "msg/Transformation.msg"
+  "msg/Unit.msg"
+  "msg/Connection.msg"
+  "msg/Target.msg"
+  "srv/GetUniqueName.srv"
+  "srv/RequestUnload.srv"
+  "srv/CreateStream.srv"
+  "srv/GetConfiguration.srv"
+  "srv/AddTransformation.srv"
+  "srv/RemoveTransformation.srv"
+  "srv/Spawn.srv"
+  "srv/Destroy.srv"
+  "srv/Configure.srv"
+  "srv/AddTarget.srv"
+  "srv/RemoveTarget.srv"
+  "srv/Refresh.srv"
+  "srv/Repair.srv"
+  "srv/GetValidInputs.srv"
+  DEPENDENCIES dyknow_nodehandle
+)
+ament_export_dependencies(rosidl_default_runtime)
+
+
+include_directories(
+  include
+  ${rclcpp_INCLUDE_DIRS}
+  ${dyknow_nodehandle_INCLUDE_DIRS}
+  ${dyknow_analytics_INCLUDE_DIRS}
+  ${rosidl_default_generators_INCLUDE_DIRS}
+  "${CMAKE_BINARY_DIR}/rosidl_generator_cpp"
+)
+
+add_library(${PROJECT_NAME}_lib
+   src/manager_componet.cpp
+   #src/configuration_manager.cpp
+   )
+
+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)
+
+
+install(TARGETS ${PROJECT_NAME}_lib
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+    DESTINATION include/${PROJECT_NAME}/
+)
 
 ament_package()
diff --git a/src/dyknow_manager/include/dyknow_manager/change.hpp b/src/dyknow_manager/include/dyknow_manager/change.hpp
new file mode 100644
index 0000000..1bf5cc3
--- /dev/null
+++ b/src/dyknow_manager/include/dyknow_manager/change.hpp
@@ -0,0 +1,115 @@
+#ifndef INCLUDE_CHANGE_HPP_
+#define INCLUDE_CHANGE_HPP_
+
+#include "environment.hpp"
+#include "transformation_spec.hpp"
+#include "target_spec.hpp"
+
+namespace dyknow {
+
+class Environment;
+
+class AbstractCommand {
+public:
+	virtual	~AbstractCommand() {}
+	virtual bool execute(Environment* envPtr) = 0;
+};
+
+class AddCommand : public AbstractCommand {
+public:
+	AddCommand(TransformationSpec spec) : spec(spec) {}
+	~AddCommand() {}
+	bool execute(Environment* envPtr);
+
+private:
+	TransformationSpec spec;
+};
+
+class RemoveCommand : public AbstractCommand {
+public:
+	RemoveCommand(std::string label) : label(label) {}
+	~RemoveCommand() {}
+	bool execute(Environment* envPtr);
+
+private:
+	std::string label;
+};
+
+class SpawnCommand : public AbstractCommand {
+public:
+	SpawnCommand(std::string name, std::string type, bool protect) : name(name), type(type), protect(protect) {}
+	~SpawnCommand() {}
+	bool execute(Environment* envPtr);
+
+private:
+	std::string name;
+	std::string type;
+	bool protect;
+};
+
+class DestroyCommand : public AbstractCommand {
+public:
+	DestroyCommand(std::string label) : label(label) {}
+	~DestroyCommand() {}
+	bool execute(Environment* envPtr);
+
+private:
+	std::string label;
+};
+
+class ConfigureCommand : public AbstractCommand {
+public:
+	ConfigureCommand(std::string name, std::map<std::string, std::string> in, std::map<std::string, std::string> out)
+	: name(name), in(in), out(out) {}
+	~ConfigureCommand() {}
+	bool execute(Environment* envPtr);
+
+private:
+	std::string name;
+	std::map<std::string, std::string> in;
+	std::map<std::string, std::string> out;
+};
+
+class AddTargetCommand : public AbstractCommand {
+public:
+	AddTargetCommand(TargetSpec spec) : spec(spec) {}
+	~AddTargetCommand() {}
+	bool execute(Environment* envPtr);
+
+private:
+	TargetSpec spec;
+};
+
+class RemoveTargetCommand : public AbstractCommand {
+public:
+	RemoveTargetCommand(std::string label) : label(label) {}
+	~RemoveTargetCommand() {}
+	bool execute(Environment* envPtr);
+
+private:
+	std::string label;
+};
+
+class RefreshCommand : public AbstractCommand {
+public:
+	RefreshCommand() {}
+	~RefreshCommand() {}
+	bool execute(Environment* envPtr);
+};
+
+class Change {
+public:
+	void add(AbstractCommand* cmd);
+	bool execute(Environment* envPtr);
+	int size() { return commands.size(); }
+
+private:
+	std::vector<AbstractCommand*> commands;
+};
+
+
+} //namespace
+
+
+
+#endif /* INCLUDE_CHANGE_HPP_ */
diff --git a/src/dyknow_manager/include/dyknow_manager/configuration_manager.hpp b/src/dyknow_manager/include/dyknow_manager/configuration_manager.hpp
new file mode 100644
index 0000000..8eebcad
--- /dev/null
+++ b/src/dyknow_manager/include/dyknow_manager/configuration_manager.hpp
@@ -0,0 +1,119 @@
+#ifndef INCLUDE_CONFIGURATION_MANAGER_HPP_
+#define INCLUDE_CONFIGURATION_MANAGER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <queue>
+
+#include "change.hpp"
+#include "environment.hpp"
+#include "configuration_planner.hpp"
+#include "transformation_spec.hpp"
+#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"
+
+namespace dyknow {
+
+
+class ConfigurationManager {
+public:
+	static const unsigned int VERBOSITY = 10;
+	static const bool EXPERIMENTAL = true;
+
+	ConfigurationManager(std::shared_ptr<dyknow::Node>& nh)
+	: nh(nh)
+	, getConfigurationService(nh->create_service<dyknow_manager::srv::GetConfiguration>("get_model", &ConfigurationManager::getConfigurationCallback))
+	, 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))
+	, destroyService(nh->create_service<dyknow_manager::srv::Destroy>("destroy", &ConfigurationManager::destroyCallback))
+	, configureService(nh->create_service<dyknow_manager::srv::Configure>("configure", &ConfigurationManager::configureCallback))
+	, addTargetService(nh->create_service<dyknow_manager::srv::AddTarget>("add_target", &ConfigurationManager::addTargetCallback))
+	, removeTargetService(nh->create_service<dyknow_manager::srv::RemoveTarget>("remove_target", &ConfigurationManager::removeTargetCallback))
+	, 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)));
+        /*
+		if(EXPERIMENTAL) {
+			createEnvironment(0, "/nodelet_manager");
+			expPopulateService = this->create_service("exp_populate", &ConfigurationManager::expPopulateCallback);
+		}
+        */
+	}
+	~ConfigurationManager() {
+	}
+	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);
+
+	bool apply(Change delta, int environment);
+	void spin();
+
+	// Thread-safe manipulation of change queue
+	bool empty();
+	std::pair<int, Change> dequeue();
+	void enqueue(std::pair<int, Change> delta);
+	int size();
+
+	void processRepairs();
+
+private:
+	std::shared_ptr<dyknow::Node> nh;
+	boost::shared_ptr< boost::thread > processThread;
+
+	std::vector<Environment> environments;
+	std::queue<std::pair<int, Change> > changeQueue;
+	ConfigurationPlanner planner;
+
+    rclcpp::Service<dyknow_manager::srv::GetConfiguration>::SharedPtr getConfigurationService;
+    rclcpp::Service<dyknow_manager::srv::AddTransformation>::SharedPtr addTransformationService;
+    rclcpp::Service<dyknow_manager::srv::RemoveTransformation>::SharedPtr removeTransformationService;
+    rclcpp::Service<dyknow_manager::srv::Spawn>::SharedPtr spawnService;
+    rclcpp::Service<dyknow_manager::srv::Destroy>::SharedPtr destroyService;
+    rclcpp::Service<dyknow_manager::srv::Configure>::SharedPtr configureService;
+    rclcpp::Service<dyknow_manager::srv::AddTarget>::SharedPtr addTargetService;
+    rclcpp::Service<dyknow_manager::srv::RemoveTarget>::SharedPtr removeTargetService;
+    rclcpp::Service<dyknow_manager::srv::Refresh>::SharedPtr refreshService;
+    rclcpp::Service<dyknow_manager::srv::Repair>::SharedPtr repairService;
+    rclcpp::Service<dyknow_manager::srv::GetValidInputs>::SharedPtr getValidInputsService;
+
+	boost::mutex deltaAccess;
+	boost::mutex envAccess;
+};
+
+
+} //namespace
+
+
+
+#endif /* INCLUDE_CONFIGURATION_MANAGER_HPP_ */
diff --git a/src/dyknow_manager/include/dyknow_manager/environment.hpp b/src/dyknow_manager/include/dyknow_manager/environment.hpp
new file mode 100644
index 0000000..671cb07
--- /dev/null
+++ b/src/dyknow_manager/include/dyknow_manager/environment.hpp
@@ -0,0 +1,103 @@
+#ifndef INCLUDE_ENVIRONMENT_HPP_
+#define INCLUDE_ENVIRONMENT_HPP_
+
+#include "util/to_string.hpp"
+#include <rclcpp/rclcpp.hpp>
+#include "dyknow_nodehandle/node.hpp"
+#include "transformation_spec.hpp"
+#include "target_spec.hpp"
+#include "compunit_helper.hpp"
+#include "feasibility_matrix.hpp"
+
+namespace dyknow {
+
+struct ComputationUnit {
+	std::string name;
+	std::string type;
+
+	ComputationUnit() {}
+
+	ComputationUnit(std::string name, std::string type) {
+		this->name = name;
+		this->type = type;
+	}
+};
+
+typedef std::map<std::string, std::string> Subscription;
+typedef std::map<std::string, std::string> Generator;
+
+struct ComputationGraph {
+	std::vector<ComputationUnit> computationUnits;
+	std::map<std::string, Subscription> subscriptionMap;
+	std::map<std::string, Generator> generatorMap;
+	std::vector<TransformationSpec> transformations;
+
+	ComputationGraph(std::vector<ComputationUnit> computationUnits, std::map<std::string, Subscription> subscriptionMap, std::map<std::string, Generator> generatorMap, std::vector<TransformationSpec> transformations) {
+		this->computationUnits = computationUnits;
+		this->subscriptionMap = subscriptionMap;
+		this->generatorMap = generatorMap;
+		this->transformations = transformations;
+	}
+};
+
+class Environment {
+public:
+	Environment() : nodeletManagerName("/nodelet_manager"), id(0), counter(0), repair(false) {}
+	Environment(std::shared_ptr<dyknow::Node> nh, unsigned int id, std::string manager) : nh(nh), nodeletManagerName(manager), id(id), counter(0), repair(false), next_transformaiton_id(1) {
+//		populate();
+	}
+	~Environment() {
+//		delete plannerPtr;
+	}
+
+	unsigned int getId() { return id; }
+	std::string getManagerName() { return nodeletManagerName; }
+
+	bool addTransformation(TransformationSpec spec);
+	bool removeTransformation(std::string label);
+	bool spawn(std::string name, std::string type, bool protect);
+	bool destroy(std::string name);
+	bool configure(std::string name, std::map<std::string, std::string> in, std::map<std::string, std::string> out);
+	bool addTarget(TargetSpec spec);
+	bool removeTarget(std::string label);
+	bool refresh();
+
+	std::vector<ComputationUnit> getComputationUnits() { return computationUnits; }
+	std::vector<TransformationSpec> getTransformations() { return transformations; }
+	std::vector<TargetSpec> getTargets() { return targets; }
+	std::map<std::string, Subscription> getSubscriptions() { return subscriptionMap; }
+	std::map<std::string, Generator> getGenerators() { return generatorMap; }
+	FeasibilityMatrix getMatrix() { return matrix; }
+	std::vector<std::string> getProtections() { return protections; }
+	ComputationGraph getComputationGraph() { return ComputationGraph(getComputationUnits(), getSubscriptions(), getGenerators(), getTransformations()); }
+
+	void updateSubscription(std::string name, Subscription subscription);
+	void updateGenerator(std::string name, Generator generator);
+
+	std::vector<Port> getValidInputs(Port source);
+
+	bool repair;
+
+	void expPopulate(int seed, int numTransform, double density, double lambda, int numTarget);
+
+private:
+	std::shared_ptr<dyknow::Node> nh;
+	unsigned int id;
+	unsigned int counter;
+	std::string nodeletManagerName;
+	std::vector<ComputationUnit> computationUnits;
+	std::vector<TransformationSpec> transformations;
+	std::vector<TargetSpec> targets;
+	std::map<std::string, Subscription> subscriptionMap;
+	std::map<std::string, Generator> generatorMap;
+	FeasibilityMatrix matrix;
+	std::vector<std::string> protections;
+	int next_transformaiton_id;
+};
+
+
+} //namespace
+
+
+
+#endif /* INCLUDE_ENVIRONMENT_HPP_ */
diff --git a/src/dyknow_manager/include/dyknow_manager/manager_componet.hpp b/src/dyknow_manager/include/dyknow_manager/manager_componet.hpp
new file mode 100644
index 0000000..f4343b5
--- /dev/null
+++ b/src/dyknow_manager/include/dyknow_manager/manager_componet.hpp
@@ -0,0 +1,26 @@
+#ifndef MANAGER_COMPONET_H_
+#define  MANAGER_COMPONET_H_
+
+#include <rclcpp/rclcpp.hpp>
+#include <dyknow_nodehandle/node.hpp>
+//#include "configuration_manager.hpp"
+#include "rclcpp_components/component_manager.hpp"
+
+
+namespace manager {
+
+class ManagerComponet {
+    public:
+        ManagerComponet(const rclcpp::NodeOptions & options);
+        
+        rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const;
+
+    private:
+        std::shared_ptr<dyknow::Node> node;
+        //std::shared_ptr<ConfigurationManager> configManPtr;
+
+};
+
+} //namespace manager
+
+#endif /* MANAGER_COMPONET_H_*/
\ No newline at end of file
diff --git a/src/dyknow_manager/include/dyknow_manager/target_spec.hpp b/src/dyknow_manager/include/dyknow_manager/target_spec.hpp
new file mode 100644
index 0000000..67c06d8
--- /dev/null
+++ b/src/dyknow_manager/include/dyknow_manager/target_spec.hpp
@@ -0,0 +1,44 @@
+#ifndef INCLUDE_TARGET_SPEC_HPP_
+#define INCLUDE_TARGET_SPEC_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include "dyknow_manager/msg/target.hpp"
+
+namespace dyknow {
+
+struct TargetSpec {
+	std::string label;
+	int id;
+	int port;
+	std::string topic;
+
+	TargetSpec() {
+		this->id = -1;
+		this->port = 0;
+		this->topic = "/result";
+		this->label = "";
+	}
+
+	TargetSpec(int id, std::string label, int port = 0, std::string topic = "/result") {
+		this->id = id;
+		this->port = port;
+		this->topic = topic;
+		this->label = label;
+	}
+
+	bool operator==(const TargetSpec& rhs) {
+		return id == rhs.id;
+	}
+};
+
+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);
+};
+
+} //namespace
+
+
+
+#endif /* INCLUDE_TARGET_SPEC_HPP_ */
diff --git a/src/dyknow_manager/include/dyknow_manager/transformation_spec.hpp b/src/dyknow_manager/include/dyknow_manager/transformation_spec.hpp
new file mode 100644
index 0000000..de23522
--- /dev/null
+++ b/src/dyknow_manager/include/dyknow_manager/transformation_spec.hpp
@@ -0,0 +1,75 @@
+#ifndef INCLUDE_TRANSFORMATION_SPEC_HPP_
+#define INCLUDE_TRANSFORMATION_SPEC_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include "dyknow_manager/msg/transformation.hpp"
+#include "dyknow_manager/msg/tag.hpp"
+#include "dyknow_manager/msg/parameter.hpp"
+
+namespace dyknow {
+
+enum TransformationType {
+	NODELET, GRAPH, JAR, UNKNOWN, COMPUNIT
+};
+
+struct TransformationParam {
+	std::string name;
+	std::string value;
+	std::string type;
+
+	TransformationParam() {}
+	TransformationParam(std::string name, std::string value, std::string type) {
+		this->name = name;
+		this->type = type;
+		this->value = value;
+	}
+};
+
+typedef std::pair<std::string, std::string> TransformationTag;
+
+
+struct TransformationSpec {
+	int id;
+	std::string label;
+	TransformationType type;
+	std::string source;
+	std::vector<TransformationParam> parameters;
+
+	std::vector<std::string> inPorts;
+	std::vector<std::string> outPorts;
+
+	std::vector<TransformationTag> tags;
+
+	double cost;
+
+	bool canRecurse;
+
+	TransformationSpec() {
+		this->id = -1;
+		this->type = UNKNOWN;
+		this->cost = 0;
+		this->canRecurse = false;
+	}
+
+	TransformationSpec(int id, std::string label, TransformationType type, double cost = 1, bool canRecurse = false) {
+		this->id = id;
+		this->label = label;
+		this->type = type;
+		this->cost = cost;
+		this->canRecurse = canRecurse;
+	}
+
+	bool operator==(const TransformationSpec& rhs) {
+	    return id == rhs.id;
+	}
+};
+
+class TransformationFactory {
+public:
+	static TransformationSpec create(std::string path);
+	static TransformationSpec create(dyknow_manager::Transformation);
+};
+
+} //namespace
+
+#endif /* INCLUDE_TRANSFORMATION_SPEC_HPP_ */
diff --git a/src/dyknow_manager/include/dyknow_manager/util/from_string.hpp b/src/dyknow_manager/include/dyknow_manager/util/from_string.hpp
new file mode 100644
index 0000000..cd0f1ce
--- /dev/null
+++ b/src/dyknow_manager/include/dyknow_manager/util/from_string.hpp
@@ -0,0 +1,27 @@
+/* -*- Mode: C++ -*- */
+
+/**
+ * @file from_string.h
+ * Helper template function casting to anything from a string,
+ *
+ * Created by: Dennis Andersson 2003-06-19
+ */
+
+
+#ifndef FROM_STRING_H
+#define FROM_STRING_H
+
+#include <sstream>
+#include <string>
+
+
+template <typename Type>
+Type from_string(const std::string& s)
+{
+  std::istringstream str(s);
+  Type var;
+  str >> var;
+  return var;
+}
+
+#endif // FROM_STRING_H
diff --git a/src/dyknow_manager/include/dyknow_manager/util/to_string.hpp b/src/dyknow_manager/include/dyknow_manager/util/to_string.hpp
new file mode 100644
index 0000000..5a28001
--- /dev/null
+++ b/src/dyknow_manager/include/dyknow_manager/util/to_string.hpp
@@ -0,0 +1,30 @@
+/* -*- Mode: C++ -*- */
+
+/**
+ * @file to_string.h
+ * Converts any object which has an operator<< defined to a string.
+ *
+ * Created by: Fredrik Heintz 2003-03-07
+ */
+
+
+#ifndef TO_STRING_H
+#define TO_STRING_H
+
+
+#include <sstream>
+
+
+template<class T>
+std::string to_string(const T& obj)
+{
+  std::ostringstream str("");
+  str << obj;
+  return str.str();
+};
+
+
+#define TO_STRING( out, msg ) std::string out; {std::ostringstream strs; strs << msg; out = std::string(strs.str());}
+
+
+#endif // !TO_STRING_H
diff --git a/src/dyknow_manager/msg/Connection.msg b/src/dyknow_manager/msg/Connection.msg
new file mode 100644
index 0000000..9e777ea
--- /dev/null
+++ b/src/dyknow_manager/msg/Connection.msg
@@ -0,0 +1,2 @@
+string port
+string topic
\ No newline at end of file
diff --git a/src/dyknow_manager/msg/Parameter.msg b/src/dyknow_manager/msg/Parameter.msg
new file mode 100644
index 0000000..367c3f8
--- /dev/null
+++ b/src/dyknow_manager/msg/Parameter.msg
@@ -0,0 +1,3 @@
+string name
+string value
+string type
\ No newline at end of file
diff --git a/src/dyknow_manager/msg/Tag.msg b/src/dyknow_manager/msg/Tag.msg
new file mode 100644
index 0000000..586b5a6
--- /dev/null
+++ b/src/dyknow_manager/msg/Tag.msg
@@ -0,0 +1,2 @@
+string port
+string tag
\ No newline at end of file
diff --git a/src/dyknow_manager/msg/Target.msg b/src/dyknow_manager/msg/Target.msg
new file mode 100644
index 0000000..d0c9cb6
--- /dev/null
+++ b/src/dyknow_manager/msg/Target.msg
@@ -0,0 +1,4 @@
+string label
+uint32 port
+string topic
+uint32 environment
\ No newline at end of file
diff --git a/src/dyknow_manager/msg/Transformation.msg b/src/dyknow_manager/msg/Transformation.msg
new file mode 100644
index 0000000..11462e6
--- /dev/null
+++ b/src/dyknow_manager/msg/Transformation.msg
@@ -0,0 +1,9 @@
+string label
+string type
+string source
+dyknow_manager/Parameter[] params
+string[] out_ports
+string[] in_ports
+dyknow_manager/Tag[] tags
+float64 cost
+uint32 environment
\ No newline at end of file
diff --git a/src/dyknow_manager/msg/Unit.msg b/src/dyknow_manager/msg/Unit.msg
new file mode 100644
index 0000000..1dd4340
--- /dev/null
+++ b/src/dyknow_manager/msg/Unit.msg
@@ -0,0 +1,5 @@
+string label
+string type
+uint32 environment
+dyknow_manager/Connection[] incoming
+dyknow_manager/Connection[] outgoing
diff --git a/src/dyknow_manager/package.xml b/src/dyknow_manager/package.xml
index 5f06783..b70646a 100644
--- a/src/dyknow_manager/package.xml
+++ b/src/dyknow_manager/package.xml
@@ -8,10 +8,16 @@
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>rosidl_default_generators</depend>
+  <depend>rclcpp_components</depend>
+  <depend>dyknow_nodehandle</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>
diff --git a/src/dyknow_manager/src/change.cpp b/src/dyknow_manager/src/change.cpp
new file mode 100644
index 0000000..75c19ab
--- /dev/null
+++ b/src/dyknow_manager/src/change.cpp
@@ -0,0 +1,55 @@
+#include "change.hpp"
+
+namespace dyknow {
+
+
+void Change::add(AbstractCommand* cmd) {
+	commands.push_back(cmd);
+}
+
+bool Change::execute(Environment* envPtr) {
+	AbstractCommand* cmdPtr;
+	bool result = true;
+	for (auto& cmdPtr : commands) {
+		if(!cmdPtr->execute(envPtr)) {
+			result = false;
+		}
+		delete cmdPtr;
+	}
+	commands.clear();
+	return result;
+}
+
+bool AddCommand::execute(Environment* envPtr) {
+	return envPtr->addTransformation(this->spec);
+}
+
+bool RemoveCommand::execute(Environment* envPtr) {
+	return envPtr->removeTransformation(this->label);
+}
+
+bool SpawnCommand::execute(Environment* envPtr) {
+	return envPtr->spawn(this->name, this->type, this->protect);
+}
+
+bool DestroyCommand::execute(Environment* envPtr) {
+	return envPtr->destroy(this->label);
+}
+
+bool ConfigureCommand::execute(Environment* envPtr) {
+	return envPtr->configure(this->name, this->in, this->out);
+}
+
+bool AddTargetCommand::execute(Environment* envPtr) {
+	return envPtr->addTarget(this->spec);
+}
+
+bool RemoveTargetCommand::execute(Environment* envPtr) {
+	return envPtr->removeTarget(this->label);
+}
+
+bool RefreshCommand::execute(Environment* envPtr) {
+	return envPtr->refresh();
+}
+
+} //namespace
diff --git a/src/dyknow_manager/src/configuration_manager.cpp b/src/dyknow_manager/src/configuration_manager.cpp
new file mode 100644
index 0000000..324aab5
--- /dev/null
+++ b/src/dyknow_manager/src/configuration_manager.cpp
@@ -0,0 +1,331 @@
+#include "configuration_manager.hpp"
+
+namespace dyknow {
+
+void ConfigurationManager::createEnvironment(int id, std::string manager) {
+	std::lock_guard<std::mutex> guard(envAccess);
+	Environment env = Environment(nh, id, manager);
+	environments.push_back(env);
+}
+
+bool ConfigurationManager::getConfigurationCallback(dyknow_manager::GetConfiguration::Request& req, dyknow_manager::GetConfiguration::Response& res) {
+	std::lock_guard<std::mutex> guard(envAccess);
+	for (auto& env : environments) {
+		std::vector<ComputationUnit> computationUnits = env.getComputationUnits();
+		for (auto& cu : computationUnits) {
+			dyknow_manager::Unit unit;
+			unit.label = cu.name;
+			unit.type = cu.type;
+			unit.environment = env.getId();
+
+			std::map<std::string, Subscription> subscriptionMap = env.getSubscriptions();
+			for (auto& pair1 : subscriptionMap[cu.name]) {
+				dyknow_manager::Connection con;
+				con.port = pair1.first;
+				con.topic = pair1.second;
+				unit.incoming.push_back(con);
+			}
+
+			std::map<std::string, Generator> generatorMap = env.getGenerators();
+			for (auto& pair2 : generatorMap[cu.name]) {
+				dyknow_manager::Connection con;
+				con.port = pair2.first;
+				con.topic = pair2.second;
+				unit.outgoing.push_back(con);
+			}
+
+			res.units.push_back(unit);
+		}
+
+		std::vector<TransformationSpec> transformations = env.getTransformations();
+		for (auto& spec : transformations) {
+			dyknow_manager::Transformation tf;
+			tf.label = spec.label;
+			tf.source = spec.source;
+			tf.in_ports = spec.inPorts;
+			tf.out_ports = spec.outPorts;
+			tf.cost = spec.cost;
+			//TODO: Type, tag and params
+			tf.environment = env.getId();
+
+			res.transformations.push_back(tf);
+		}
+
+		std::vector<TargetSpec> targets = env.getTargets();
+		for (auto& targetSpec : targets) {
+			dyknow_manager::Target tgt;
+			tgt.label = targetSpec.label;
+			tgt.port = targetSpec.port;
+			tgt.topic = targetSpec.topic;
+			tgt.environment = env.getId();
+
+			res.targets.push_back(tgt);
+		}
+	}
+	return true;
+}
+
+bool ConfigurationManager::addTransformationCallback(dyknow_manager::AddTransformation::Request& req, dyknow_manager::AddTransformation::Response& res) {
+	Change delta;
+	for (auto& tfSpec : req.transformations) {
+		TransformationSpec spec = TransformationFactory::create(tfSpec);
+		AddCommand* cmdPtr = new AddCommand(spec);
+		delta.add(cmdPtr);
+	}
+
+	enqueue(std::pair<int, Change>(req.envId, delta));
+	res.success = true;
+	return true;
+}
+
+bool ConfigurationManager::removeTransformationCallback(dyknow_manager::RemoveTransformation::Request& req, dyknow_manager::RemoveTransformation::Response& res) {
+	Change delta;
+	for (auto& label : req.transformations) {
+		RemoveCommand* cmdPtr = new RemoveCommand(label);
+		delta.add(cmdPtr);
+	}
+
+	enqueue(std::pair<int, Change>(req.envId, delta));
+	res.success = true;
+	return true;
+}
+
+bool ConfigurationManager::spawnCallback(dyknow_manager::Spawn::Request& req, 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;
+}
+
+bool ConfigurationManager::destroyCallback(dyknow_manager::Destroy::Request& req, 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;
+}
+
+bool ConfigurationManager::configureCallback(dyknow_manager::Configure::Request& req, 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;
+	}
+
+	std::map<std::string, std::string> in;
+	for(size_t i = 0; i < req.config.in_channels.size(); ++i) {
+		if(req.config.in_channels[i].empty()) { continue; }
+		in.emplace(req.config.in_channels[i], req.config.in_names[i]);
+	}
+
+	std::map<std::string, std::string> out;
+	for(size_t i = 0; i < req.config.out_channels.size(); ++i) {
+		if(req.config.out_channels[i].empty()) { continue; }
+		out.emplace(req.config.out_channels[i], req.config.out_names[i]);
+	}
+
+	ConfigureCommand* cmdPtr = new ConfigureCommand(req.name, in, out);
+	delta.add(cmdPtr);
+	enqueue(std::pair<int, Change>(req.envId, delta));
+	res.success = true;
+	return true;
+}
+
+bool ConfigurationManager::addTargetCallback(dyknow_manager::AddTarget::Request& req, dyknow_manager::AddTarget::Response& res) {
+	Change delta;
+	for (auto& target : req.targets) {
+		TargetSpec spec = TargetFactory::create(target);
+		AddTargetCommand* cmdPtr = new AddTargetCommand(spec);
+		delta.add(cmdPtr);
+	}
+
+	enqueue(std::pair<int, Change>(req.envId, delta));
+	res.success = true;
+	return true;
+}
+
+bool ConfigurationManager::removeTargetCallback(dyknow_manager::RemoveTarget::Request& req, dyknow_manager::RemoveTarget::Response& res) {
+	Change delta;
+	for (auto& label : req.targets) {
+		RemoveTargetCommand* cmdPtr = new RemoveTargetCommand(label);
+		delta.add(cmdPtr);
+	}
+
+	enqueue(std::pair<int, Change>(req.envId, delta));
+	res.success = true;
+	return true;
+}
+
+bool ConfigurationManager::refreshCallback(dyknow_manager::Refresh::Request& req, 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;
+}
+
+bool ConfigurationManager::repairCallback(dyknow_manager::Repair::Request& req, 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) {
+		return req.envId == env.getId();
+	});
+
+	if(iter != environments.end()) {
+		iter->repair = true;
+		res.success = true;
+		return true;
+	}
+
+	ROS_WARN("Unable to find environment: %i", req.envId);
+	res.success = false;
+	return false;
+}
+
+bool ConfigurationManager::getValidInputsCallback(dyknow_manager::GetValidInputs::Request& req, 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) {
+		return req.env == env.getId();
+	});
+
+	if(iter != environments.end()) {
+		int id = -1;
+		for(size_t i = 0; i < iter->getMatrix().library.size(); ++i) {
+			if(iter->getMatrix().library[i].label == req.transformation) {
+				id = iter->getMatrix().library[i].id;
+				break;
+			}
+		}
+
+		if(id < 0) {
+			ROS_WARN("Unable to find transformation with label %s; skipping", req.transformation.c_str());
+			res.success = false;
+			return true;
+		}
+
+		for (auto& port : iter->getValidInputs(Port(id, req.port-1))) {
+			ROS_INFO("getValidInputs");
+			TransformationSpec spec = iter->getMatrix().decode(port);
+
+			dyknow_manager::Transformation tf;
+			tf.cost = spec.cost;
+			tf.environment = req.env;
+			tf.in_ports = spec.inPorts;
+			tf.out_ports = spec.outPorts;
+			tf.label = spec.label;
+			tf.source = spec.source;
+			tf.type = spec.type == NODELET ? "nodelet" : "unknown";
+
+			for (auto& param : spec.parameters) {
+				dyknow_manager::Parameter elem;
+				elem.name = param.name;
+				elem.type = param.type;
+				elem.value = param.value;
+				tf.params.push_back(elem);
+			}
+
+			for (auto& tag : spec.tags) {
+				dyknow_manager::Tag elem;
+				elem.port = tag.first;
+				elem.tag = tag.second;
+				tf.tags.push_back(elem);
+			}
+
+			res.transformations.push_back(tf);
+			res.ports.push_back(port.second);
+		}
+
+		res.success = true;
+		return true;
+	}
+
+	ROS_WARN("Unable to find environment: %i", req.env);
+	return true;
+}
+
+bool ConfigurationManager::expPopulateCallback(dyknow_manager::ExpPopulate::Request& req, 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) {
+		return req.env == env.getId();
+	});
+
+	if(iter != environments.end()) {
+		iter->expPopulate(req.seed, req.numTransform, req.density, req.lambda, req.numTarget);
+		res.success = true;
+		return true;
+	}
+
+	ROS_WARN("Unable to find environment: %i", req.env);
+	return true;
+}
+
+void ConfigurationManager::spin() {
+	while(ros::ok()) {
+		processRepairs();
+
+		if(empty()) {
+			ros::Duration(1.0).sleep();
+		} else {
+			std::pair<int, Change> changePair = dequeue();
+			apply(changePair.second, changePair.first);
+		}
+	}
+}
+
+bool 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) {
+		return environment == env.getId();
+	});
+
+	if(iter != environments.end()) {
+		return delta.execute(&*iter);
+	}
+
+	ROS_WARN("Unable to find environment: %i", environment);
+	return false;
+}
+
+bool ConfigurationManager::empty() {
+	std::lock_guard<std::mutex> guard(deltaAccess);
+	return changeQueue.empty();
+}
+
+int ConfigurationManager::size() {
+	std::lock_guard<std::mutex> guard(deltaAccess);
+	return changeQueue.size();
+}
+
+std::pair<int, Change> ConfigurationManager::dequeue() {
+	std::lock_guard<std::mutex> guard(deltaAccess);
+	std::pair<int, Change> elem = changeQueue.front();
+	changeQueue.pop();
+	return elem;
+}
+
+void ConfigurationManager::enqueue(std::pair<int, Change> delta) {
+	std::lock_guard<std::mutex> guard(deltaAccess);
+	changeQueue.push(delta);
+}
+
+void ConfigurationManager::processRepairs() {
+	std::lock_guard<std::mutex> guard(envAccess);
+	for(auto& env : environments) {
+		if(env.repair) {
+			Change delta = planner.optimise(&env, VERBOSITY);
+			enqueue(std::pair<int, Change>(env.getId(), delta));
+			env.repair = false;
+		}
+	}
+}
+
+} //namespace
diff --git a/src/dyknow_manager/src/environment.cpp b/src/dyknow_manager/src/environment.cpp
new file mode 100644
index 0000000..b5d1c94
--- /dev/null
+++ b/src/dyknow_manager/src/environment.cpp
@@ -0,0 +1,160 @@
+#include "environment.hpp"
+
+namespace dyknow {
+
+bool Environment::addTransformation(TransformationSpec spec) {
+	ROS_INFO("Adding transformation %s (cost: %f)", spec.label.c_str(), spec.cost);
+	//TODO: Set transformation ID if -1
+	if (spec.id < 0){
+		spec.id = next_transformaiton_id;
+		next_transformaiton_id++;
+	}
+	transformations.push_back(spec);
+	return true;
+}
+
+bool Environment::removeTransformation(std::string label) {
+	ROS_INFO("Removing transformation %s", label.c_str());
+	std::string s;
+
+	// Remove transformation from library and invalidate in matrix
+	auto iter = std::remove_if(transformations.begin(), transformations.end(), [&label](const TransformationSpec& ts) {
+		return ts.label == label;
+	});
+
+	if (iter != transformations.end()) {
+		transformations.erase(iter, transformations.end());
+		matrix.invalidate(label);
+	}
+
+	return true;
+}
+
+bool Environment::spawn(std::string name, std::string type, bool protect) {
+	auto iter = std::find_if(transformations.begin(), transformations.end(), [&type](const TransformationSpec& ts) {
+		return ts.label == type;
+	});
+
+	if (iter != transformations.end()) {
+		std::vector<std::string> args;
+		for (const auto& param : iter->parameters) {
+			args.push_back(param.value); //FIXME: The interface currently ignores parameter names and types
+		}
+
+		if (CompUnitHelper::spawn(nh, nodeletManagerName, name, iter->source, args)) {
+			ComputationUnit cu(name, type);
+			computationUnits.push_back(cu);
+			subscriptionMap.emplace(name, Subscription());
+			generatorMap.emplace(name, Generator());
+			if (protect) {
+				protections.push_back(name);
+				ROS_INFO("** %s is now protected **", name.c_str());
+			}
+			return true;
+		}
+	}
+	return false;
+}
+
+bool Environment::destroy(std::string name) {
+	auto iter = std::find_if(computationUnits.begin(), computationUnits.end(), [&name](const ComputationUnit& cu) {
+		return cu.name == name;
+	});
+
+	if (iter != computationUnits.end()) {
+		if (CompUnitHelper::destroy(nh, nodeletManagerName, name)) {
+			computationUnits.erase(iter);
+			subscriptionMap.erase(name);
+			generatorMap.erase(name);
+			return true;
+		}
+	} else {
+		ROS_WARN("Unrecognised CU registered for unloading: %s", name.c_str());
+	}
+
+	return false;
+}
+
+bool Environment::configure(std::string name, std::map<std::string, std::string> in, std::map<std::string, std::string> out) {
+	auto iter = std::find_if(computationUnits.begin(), computationUnits.end(), [&name](const ComputationUnit& cu) {
+		return cu.name == name;
+	});
+
+	if (iter != computationUnits.end()) {
+		if (CompUnitHelper::configure(nh, nodeletManagerName, name, in, out)) {
+			updateSubscription(name, in);
+			updateGenerator(name, out);
+			return true;
+		}
+	} else {
+		ROS_WARN("Unrecognised CU registered for reconfiguration: %s", name.c_str());
+	}
+
+	return false;
+}
+
+bool Environment::addTarget(TargetSpec spec) {
+	if (spec.id < 0) {
+		auto iter = std::find_if(transformations.begin(), transformations.end(), [&spec](const TransformationSpec& ts) {
+			return ts.label == spec.label;
+		});
+
+		if (iter != transformations.end()) {
+			spec.id = iter->id;
+			targets.push_back(spec);
+			this->repair = true;
+			ROS_INFO("Adding target %i (%s)", spec.id, spec.label.c_str());
+			return true;
+		} else {
+			ROS_WARN("Could not find identifier for target %s", spec.label.c_str());
+			return false;
+		}
+	} else {
+		targets.push_back(spec);
+		this->repair = true;
+		ROS_INFO("Adding target %i (%s)", spec.id, spec.label.c_str());
+		return true;
+	}
+}
+
+bool Environment::removeTarget(std::string label) {
+	ROS_INFO("Removing target %s", label.c_str());
+
+	auto iter = std::remove_if(targets.begin(), targets.end(), [&label](const TargetSpec& ts) {
+		return ts.label == label;
+	});
+
+	if (iter != targets.end()) {
+		targets.erase(iter, targets.end());
+	} else {
+		ROS_WARN("Unrecognised target registered for removal: %s", label.c_str());
+	}
+
+	return true;
+}
+
+bool Environment::refresh() {
+	ROS_INFO("Refreshing feasibility matrix");
+	this->matrix = FeasibilityMatrix(this->transformations, 0.01);
+	return true;
+}
+
+void Environment::updateSubscription(std::string name, Subscription subscription) {
+	for (const auto& elem : subscription) {
+		(subscriptionMap[name])[elem.first] = elem.second;
+	}
+}
+
+void Environment::updateGenerator(std::string name, Generator generator) {
+	for (const auto& elem : generator) {
+		(generatorMap[name])[elem.first] = elem.second;
+	}
+}
+
+std::vector<Port> Environment::getValidInputs(Port source) {
+	return matrix.getValid(source);
+}
+
+// expPopulate couldbe added here!
+
+} //namespace
diff --git a/src/dyknow_manager/src/manager_componet.cpp b/src/dyknow_manager/src/manager_componet.cpp
new file mode 100644
index 0000000..b10cab3
--- /dev/null
+++ b/src/dyknow_manager/src/manager_componet.cpp
@@ -0,0 +1,15 @@
+#include <dyknow_manager/manager_componet.hpp>
+#include <rclcpp_components/register_node_macro.hpp>
+
+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));
+	
+		}
+
+}
+
+RCLCPP_COMPONENTS_REGISTER_NODE(manager::ManagerComponet)
\ No newline at end of file
diff --git a/src/dyknow_manager/src/manager_node.cpp b/src/dyknow_manager/src/manager_node.cpp
deleted file mode 100644
index 87873cc..0000000
--- a/src/dyknow_manager/src/manager_node.cpp
+++ /dev/null
@@ -1,10 +0,0 @@
-#include <cstdio>
-
-int main(int argc, char ** argv)
-{
-  (void) argc;
-  (void) argv;
-
-  printf("hello world dyknow_manager package\n");
-  return 0;
-}
diff --git a/src/dyknow_manager/src/target_spec.cpp b/src/dyknow_manager/src/target_spec.cpp
new file mode 100644
index 0000000..7924737
--- /dev/null
+++ b/src/dyknow_manager/src/target_spec.cpp
@@ -0,0 +1,18 @@
+#include "target_spec.hpp"
+
+namespace dyknow {
+
+TargetSpec TargetFactory::create(int id, std::string label, int port, std::string topic) {
+	return TargetSpec(id, label, port, topic);
+}
+
+TargetSpec TargetFactory::create(dyknow_manager::Target target) {
+	TargetSpec spec;
+	spec.label = target.label;
+	spec.port = target.port;
+	spec.topic = target.topic;
+
+	return spec;
+}
+
+}
diff --git a/src/dyknow_manager/src/transformation_spec.cpp b/src/dyknow_manager/src/transformation_spec.cpp
new file mode 100644
index 0000000..d548bfd
--- /dev/null
+++ b/src/dyknow_manager/src/transformation_spec.cpp
@@ -0,0 +1,39 @@
+#include "transformation_spec.hpp"
+
+namespace dyknow {
+
+TransformationSpec TransformationFactory::create(std::string path) {
+	//TODO: Parser that takes an ontology file and produces a set of transformation specs
+	return TransformationSpec();
+}
+
+TransformationSpec TransformationFactory::create(dyknow_manager::Transformation tfMsg) {
+	TransformationSpec spec;
+	spec.label = tfMsg.label;
+	if (tfMsg.type.compare("nodelet") == 0) {
+		spec.type = NODELET;
+	} else {
+		spec.type = UNKNOWN;
+	}
+
+	spec.source = tfMsg.source;
+	spec.outPorts = tfMsg.out_ports;
+	spec.inPorts = tfMsg.in_ports;
+
+	for (const auto& tag : tfMsg.tags) {
+		std::pair<std::string, std::string> pair(tag.port, tag.tag);
+		spec.tags.push_back(pair);
+	}
+
+	for (const auto& param : tfMsg.params) {
+		TransformationParam p;
+		p.name = param.name;
+		p.type = param.type;
+		p.value = param.value;
+		spec.parameters.push_back(p);
+	}
+
+	return spec;
+}
+
+} //namespace
diff --git a/src/dyknow_manager/srv/AddTarget.srv b/src/dyknow_manager/srv/AddTarget.srv
new file mode 100644
index 0000000..2f00dac
--- /dev/null
+++ b/src/dyknow_manager/srv/AddTarget.srv
@@ -0,0 +1,4 @@
+dyknow_manager/Target[] targets
+uint32 envid
+---
+bool success
\ No newline at end of file
diff --git a/src/dyknow_manager/srv/AddTransformation.srv b/src/dyknow_manager/srv/AddTransformation.srv
new file mode 100644
index 0000000..a98a4c2
--- /dev/null
+++ b/src/dyknow_manager/srv/AddTransformation.srv
@@ -0,0 +1,4 @@
+dyknow_manager/Transformation[] transformations
+uint32 envid
+---
+bool success
\ No newline at end of file
diff --git a/src/dyknow_manager/srv/Configure.srv b/src/dyknow_manager/srv/Configure.srv
new file mode 100644
index 0000000..513003b
--- /dev/null
+++ b/src/dyknow_manager/srv/Configure.srv
@@ -0,0 +1,5 @@
+string name
+dyknow_nodehandle/Configuration config
+uint32 envid
+---
+bool success
\ No newline at end of file
diff --git a/src/dyknow_manager/srv/CreateStream.srv b/src/dyknow_manager/srv/CreateStream.srv
new file mode 100644
index 0000000..eb23bb3
--- /dev/null
+++ b/src/dyknow_manager/srv/CreateStream.srv
@@ -0,0 +1,4 @@
+string specification
+---
+bool success
+string topic
diff --git a/src/dyknow_manager/srv/Destroy.srv b/src/dyknow_manager/srv/Destroy.srv
new file mode 100644
index 0000000..f75f3ab
--- /dev/null
+++ b/src/dyknow_manager/srv/Destroy.srv
@@ -0,0 +1,4 @@
+string name
+uint32 envid
+---
+bool success
\ No newline at end of file
diff --git a/src/dyknow_manager/srv/ExpPopulate.srv b/src/dyknow_manager/srv/ExpPopulate.srv
new file mode 100644
index 0000000..23867e9
--- /dev/null
+++ b/src/dyknow_manager/srv/ExpPopulate.srv
@@ -0,0 +1,9 @@
+# Experimental service!
+uint32 env
+uint32 seed
+uint32 numTransform=1000
+float32 lambda=2
+uint32 numTarget
+float32 density=0.01
+---
+bool success
\ No newline at end of file
diff --git a/src/dyknow_manager/srv/GetConfiguration.srv b/src/dyknow_manager/srv/GetConfiguration.srv
new file mode 100644
index 0000000..b5b607f
--- /dev/null
+++ b/src/dyknow_manager/srv/GetConfiguration.srv
@@ -0,0 +1,4 @@
+---
+dyknow_manager/Unit[] units
+dyknow_manager/Transformation[] transformations
+dyknow_manager/Target[] targets
\ No newline at end of file
diff --git a/src/dyknow_manager/srv/GetUniqueName.srv b/src/dyknow_manager/srv/GetUniqueName.srv
new file mode 100644
index 0000000..22f4c6d
--- /dev/null
+++ b/src/dyknow_manager/srv/GetUniqueName.srv
@@ -0,0 +1,4 @@
+string base
+---
+string name
+bool success
\ No newline at end of file
diff --git a/src/dyknow_manager/srv/GetValidInputs.srv b/src/dyknow_manager/srv/GetValidInputs.srv
new file mode 100644
index 0000000..23195a1
--- /dev/null
+++ b/src/dyknow_manager/srv/GetValidInputs.srv
@@ -0,0 +1,7 @@
+string transformation
+uint32 port
+uint32 env
+---
+bool success
+Transformation[] transformations
+uint32[] ports
\ No newline at end of file
diff --git a/src/dyknow_manager/srv/Refresh.srv b/src/dyknow_manager/srv/Refresh.srv
new file mode 100644
index 0000000..265dfd6
--- /dev/null
+++ b/src/dyknow_manager/srv/Refresh.srv
@@ -0,0 +1,3 @@
+uint32 envid
+---
+bool success
\ No newline at end of file
diff --git a/src/dyknow_manager/srv/RemoveTarget.srv b/src/dyknow_manager/srv/RemoveTarget.srv
new file mode 100644
index 0000000..f0abc15
--- /dev/null
+++ b/src/dyknow_manager/srv/RemoveTarget.srv
@@ -0,0 +1,4 @@
+string[] targets
+uint32 envid
+---
+bool success
\ No newline at end of file
diff --git a/src/dyknow_manager/srv/RemoveTransformation.srv b/src/dyknow_manager/srv/RemoveTransformation.srv
new file mode 100644
index 0000000..bd9024a
--- /dev/null
+++ b/src/dyknow_manager/srv/RemoveTransformation.srv
@@ -0,0 +1,4 @@
+string[] transformations
+uint32 envid
+---
+bool success
\ No newline at end of file
diff --git a/src/dyknow_manager/srv/Repair.srv b/src/dyknow_manager/srv/Repair.srv
new file mode 100644
index 0000000..265dfd6
--- /dev/null
+++ b/src/dyknow_manager/srv/Repair.srv
@@ -0,0 +1,3 @@
+uint32 envid
+---
+bool success
\ No newline at end of file
diff --git a/src/dyknow_manager/srv/RequestUnload.srv b/src/dyknow_manager/srv/RequestUnload.srv
new file mode 100644
index 0000000..689ed77
--- /dev/null
+++ b/src/dyknow_manager/srv/RequestUnload.srv
@@ -0,0 +1,3 @@
+string id
+---
+bool success
diff --git a/src/dyknow_manager/srv/Spawn.srv b/src/dyknow_manager/srv/Spawn.srv
new file mode 100644
index 0000000..b3b79eb
--- /dev/null
+++ b/src/dyknow_manager/srv/Spawn.srv
@@ -0,0 +1,6 @@
+string name
+string type
+bool protect
+uint32 envid
+---
+bool success
\ No newline at end of file
diff --git a/src/dyknow_nodehandle/package.xml b/src/dyknow_nodehandle/package.xml
index 1546ced..f97f0a1 100644
--- a/src/dyknow_nodehandle/package.xml
+++ b/src/dyknow_nodehandle/package.xml
@@ -14,7 +14,6 @@
   <depend>rosidl_default_generators</depend>
   <depend>dyknow_analytics</depend>
 
-  
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
-- 
GitLab