From 040eef64419d8467f5238a15d439b853d2dd9cb0 Mon Sep 17 00:00:00 2001
From: leolol <leoja464@student.liu.se>
Date: Thu, 30 Jan 2025 22:05:31 +0100
Subject: [PATCH] it compiled (somehow)

---
 src/dyknow_echo/CMakeLists.txt                |   2 +
 .../include/dyknow_echo/echoComposition.hpp   |   1 -
 src/dyknow_echo/src/echoComposition.cpp       |  51 ++---
 src/dyknow_manager/CMakeLists.txt             |   5 +-
 .../dyknow_manager/feasibility_matrix.hpp     |   1 +
 .../dyknow_manager/manager_componet.hpp       |   1 -
 src/dyknow_manager/src/compunit_helper.cpp    | 151 +++++++-------
 src/dyknow_manager/src/configuration_node.cpp |  10 +-
 .../src/configuration_planner.cpp             | 195 +++++++++---------
 src/dyknow_manager/src/feasibility_matrix.cpp |  10 +-
 src/dyknow_manager/src/manager_componet.cpp   |   6 +-
 src/dyknow_manager/src/target_spec.cpp        |   6 +-
 12 files changed, 215 insertions(+), 224 deletions(-)

diff --git a/src/dyknow_echo/CMakeLists.txt b/src/dyknow_echo/CMakeLists.txt
index 25d31a6..1c6d19c 100644
--- a/src/dyknow_echo/CMakeLists.txt
+++ b/src/dyknow_echo/CMakeLists.txt
@@ -12,6 +12,7 @@ find_package(rclcpp_components REQUIRED)
 find_package(dyknow_nodehandle REQUIRED)
 find_package(dyknow_analytics REQUIRED)
 find_package(composition_interfaces REQUIRED)
+find_package(example_interfaces REQUIRED)
 
 # include_directories(
 #   include
@@ -37,6 +38,7 @@ ament_target_dependencies(dyknow_echo_component
   dyknow_nodehandle
   dyknow_analytics
   composition_interfaces
+  example_interfaces
 )
 
 # rclcpp_components_register_node(dyknow_echo_component
diff --git a/src/dyknow_echo/include/dyknow_echo/echoComposition.hpp b/src/dyknow_echo/include/dyknow_echo/echoComposition.hpp
index 845c9ab..8ddf821 100644
--- a/src/dyknow_echo/include/dyknow_echo/echoComposition.hpp
+++ b/src/dyknow_echo/include/dyknow_echo/echoComposition.hpp
@@ -13,7 +13,6 @@ class EchoComp {
 		EchoComp(const rclcpp::NodeOptions & options);
 	    void callback(std::shared_ptr<dyknow_interfaces::msg::Sample>& sample);
 
-
 		rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const;
 
 	private:
diff --git a/src/dyknow_echo/src/echoComposition.cpp b/src/dyknow_echo/src/echoComposition.cpp
index 5af9000..5707c36 100644
--- a/src/dyknow_echo/src/echoComposition.cpp
+++ b/src/dyknow_echo/src/echoComposition.cpp
@@ -1,6 +1,8 @@
 #include "dyknow_echo/echoComposition.hpp"
 #include <rclcpp_components/register_node_macro.hpp>
 #include <composition_interfaces/srv/load_node.hpp>  
+#include "example_interfaces/srv/add_two_ints.hpp"
+
 
 
 namespace echo {
@@ -13,7 +15,7 @@ namespace echo {
         }
         node->connectInput("_1", "in");
         pub = node->create_publisher<dyknow_interfaces::msg::Sample>(1000);
-        node->connectOutput("/1", "out");
+        node->connectOutput("/1", "in");
     }
 
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr EchoComp::get_node_base_interface() const
@@ -27,44 +29,31 @@ namespace echo {
         //copy.header = sample->header;
         //copy.valid_time = sample->valid_time;
         copy.test = sample->test;
-        RCLCPP_INFO(node->get_logger(), "I LIVE, AM I ALIVE, I AM MEEEEE ");
-
-        rclcpp::Client<composition_interfaces::srv::LoadNode>::SharedPtr client = node->create_client<composition_interfaces::srv::LoadNode>("load_node");
 
-	    auto loadComponentService = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
 
-	    loadComponentService->plugin_name = "AAAAAAA";
-
-        // Wait for the service to be available
-        if (!client->wait_for_service(std::chrono::seconds(10))) // fun segmentation fault after this timer is up
+        // if the client is incorectly defined here then we will get a segmentation fault, on top of that if the service is not available then we just crash 
+        // ros2 service call /ComponentManager/_container/load_node composition_interfaces/srv/LoadNode "{ package_name: "nav2_planner", plugin_name: "nav2_planner::PlannerServer", parameters: [ {name: "global_costmap.global_costmap.resolution", value: { type: 3, double_value: "1.23"}}]}"
+        // ros2 service call /ComponentManager/_container/load_node composition_interfaces/srv/LoadNode "{ package_name: "dyknow_echo", plugin_name: "echo::EchoComp", parameters: []}"
+        rclcpp::Client<composition_interfaces::srv::LoadNode>::SharedPtr client = node->create_client<composition_interfaces::srv::LoadNode>("ComponentManager/_container/load_node"); 
+        //rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client = node->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
+	    
+        auto loadComponentService = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
+        //auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
+        // request->a = 1;
+        // request->b = 2;
+
+	    loadComponentService->package_name = "dyknow_echo";
+        loadComponentService->plugin_name = "echo::EchoComp";
+        
+        if (!client->wait_for_service(std::chrono::seconds(10))) 
         {
             RCLCPP_ERROR(node->get_logger(), "Service %s/load_nodelet not available");
             return;
         }   
-        RCLCPP_INFO(node->get_logger(), "");
+        RCLCPP_INFO(node->get_logger(), "AAAAAAAAA");
 
 
-        // Call the service
-        try
-        {
-            auto result = client->async_send_request(loadComponentService);
-            if (rclcpp::spin_until_future_complete(this->node, result) ==
-                rclcpp::FutureReturnCode::SUCCESS)
-            {
-                RCLCPP_INFO(this->node->get_logger(), "Successfully spawned nodelet");
-                return;
-            }
-            else
-            {
-                RCLCPP_ERROR(this->node->get_logger(), "Unable to call service for with source ");
-                return;
-            }
-        }
-        catch (const std::exception &e)
-        {
-            RCLCPP_ERROR(this->node->get_logger(), "Exception while calling service: %s", e.what());
-            return;
-        }
+        auto result = client->async_send_request(loadComponentService);
 
         pub.publish(copy);
     }
diff --git a/src/dyknow_manager/CMakeLists.txt b/src/dyknow_manager/CMakeLists.txt
index 91df589..5a936cf 100644
--- a/src/dyknow_manager/CMakeLists.txt
+++ b/src/dyknow_manager/CMakeLists.txt
@@ -22,7 +22,10 @@ add_library(${PROJECT_NAME} SHARED
    src/environment.cpp
    src/change.cpp
    src/compunit_helper.cpp
-   
+   src/configuration_planner.cpp
+   src/feasibility_matrix.cpp
+   src/configuration_node.cpp
+   src/target_spec.cpp
 
 ) # this should be all the source files that are requierd for compiling the lib
 
diff --git a/src/dyknow_manager/include/dyknow_manager/feasibility_matrix.hpp b/src/dyknow_manager/include/dyknow_manager/feasibility_matrix.hpp
index 383d4f4..dc72160 100644
--- a/src/dyknow_manager/include/dyknow_manager/feasibility_matrix.hpp
+++ b/src/dyknow_manager/include/dyknow_manager/feasibility_matrix.hpp
@@ -3,6 +3,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include "transformation_spec.hpp"
+#include <climits>
 
 namespace dyknow {
 
diff --git a/src/dyknow_manager/include/dyknow_manager/manager_componet.hpp b/src/dyknow_manager/include/dyknow_manager/manager_componet.hpp
index f336019..d2bb1b8 100644
--- a/src/dyknow_manager/include/dyknow_manager/manager_componet.hpp
+++ b/src/dyknow_manager/include/dyknow_manager/manager_componet.hpp
@@ -18,7 +18,6 @@ class ManagerComponet {
     private:
         std::shared_ptr<dyknow::Node> node;
         std::shared_ptr<dyknow::ConfigurationManager> configManPtr;
-
 };
 
 } //namespace manager
diff --git a/src/dyknow_manager/src/compunit_helper.cpp b/src/dyknow_manager/src/compunit_helper.cpp
index 11bf653..d04f311 100644
--- a/src/dyknow_manager/src/compunit_helper.cpp
+++ b/src/dyknow_manager/src/compunit_helper.cpp
@@ -8,89 +8,86 @@ bool CompUnitHelper::spawn(std::shared_ptr<dyknow::Node> nh, const std::string&
 		return false;
 	}
 
-
-	// ros2 run rclcpp_components component_container
-	// ros2 service call /ComponentManager/_container/load_node composition_interfaces/srv/LoadNode "{ package_name: "nav2_planner", plugin_name: "nav2_planner::PlannerServer", parameters: [ {name: "global_costmap.global_costmap.resolution", value: { type: 3, double_value: "1.23"}}]}"
-	// above is the special sause that we will use to make this work, we need to at this point call a service from a component that we can use to lanuch another component 
-
-	rclcpp::Client<composition_interfaces::srv::LoadNode>::SharedPtr client = nh->create_client<composition_interfaces::srv::LoadNode>("load_node");
-
-	//composition_interfaces::srv::LoadNode loadComponentService;
-	auto loadComponentService = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
-
-	loadComponentService->plugin_name = name;
-	//loadComponentService.request.type = source;
-	//loadComponentService.request.my_argv = args;
-
-	if (client.call(loadNodeletService))
-	{
-		RCLCPP_INFO(nh->get_logger(), "Successfully spawned nodelet %s", name.c_str());
-		return true;
-	}
-	else
-	{
-		RCLCPP_ERROR(nh->get_logger(), "Unable to call service for %s with source %s", name.c_str(), source.c_str());
-		return false;
-	}
+    rclcpp::Client<composition_interfaces::srv::LoadNode>::SharedPtr client = nh->create_client<composition_interfaces::srv::LoadNode>("ComponentManager/_container/load_node"); 
+
+	auto loadComponentRequest = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
+
+	loadComponentRequest->plugin_name = name;
+	//loadComponentRequest.request.type = source;
+	//loadComponentRequest.request.my_argv = args;
+
+	// if (client->async_send_request(loadComponentRequest))
+	// {
+	// 	RCLCPP_INFO(nh->get_logger(), "Successfully spawned nodelet %s", name.c_str());
+	// 	return true;
+	// }
+	// else
+	// {
+	// 	RCLCPP_ERROR(nh->get_logger(), "Unable to call service for %s with source %s", name.c_str(), source.c_str());
+	// 	return false;
+	// }
+	return false;
 }
 
 bool CompUnitHelper::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) {
-	ros::ServiceClient client = nh.serviceClient<dyknow_nodehandle::Configure>("/"+name+"/set_config");
-	dyknow_nodehandle::Configure configureService;
-
-	//std::pair<std::string, std::string> elem;
-	for_each(outputMap.begin(), outputMap.end(), [&configureService](std::pair<std::string, std::string> elem) {
-		configureService.request.config.out_names.push_back(elem.second);
-		configureService.request.config.out_channels.push_back(elem.first);
-	});
-
-	for_each(inputMap.begin(), inputMap.end(), [&configureService](std::pair<std::string, std::string> elem) {
-		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) {
-			RCLCPP_ERROR(nh->get_logger(), "Failed to configure nodelet %s", name.c_str());
-		}
-		else {
-			RCLCPP_INFO(nh->get_logger(), "Successfully configured nodelet %s", name.c_str());
-		}
-		return true;
-	}
-	else
-	{
-		RCLCPP_ERROR(nh->get_logger(), "Unable to configure nodelet %s", name.c_str());
-		return false;
-	}
+	return false;
+	// ros::ServiceClient client = nh.serviceClient<dyknow_nodehandle::Configure>("/"+name+"/set_config");
+	// dyknow_nodehandle::Configure configureService;
+
+	// //std::pair<std::string, std::string> elem;
+	// for_each(outputMap.begin(), outputMap.end(), [&configureService](std::pair<std::string, std::string> elem) {
+	// 	configureService.request.config.out_names.push_back(elem.second);
+	// 	configureService.request.config.out_channels.push_back(elem.first);
+	// });
+
+	// for_each(inputMap.begin(), inputMap.end(), [&configureService](std::pair<std::string, std::string> elem) {
+	// 	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) {
+	// 		RCLCPP_ERROR(nh->get_logger(), "Failed to configure nodelet %s", name.c_str());
+	// 	}
+	// 	else {
+	// 		RCLCPP_INFO(nh->get_logger(), "Successfully configured nodelet %s", name.c_str());
+	// 	}
+	// 	return true;
+	// }
+	// else
+	// {
+	// 	RCLCPP_ERROR(nh->get_logger(), "Unable to configure nodelet %s", name.c_str());
+	// 	return false;
+	// }
 }
 
 bool CompUnitHelper::destroy(std::shared_ptr<dyknow::Node> 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) {
-			RCLCPP_ERROR(nh->get_logger(), "Unable to unload nodelet %s: failure while unloading", name.c_str());
-			return false;
-		}
-		else {
-			RCLCPP_INFO(nh->get_logger(), "Successfully unloaded nodelet %s", name.c_str());
-			return true;
-		}
-	}
-	else
-	{
-		RCLCPP_ERROR(nh->get_logger(), "Unable to unload nodelet %s: service unavailable", name.c_str());
-		return false;
-	}
+	return false;
+	// 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) {
+	// 		RCLCPP_ERROR(nh->get_logger(), "Unable to unload nodelet %s: failure while unloading", name.c_str());
+	// 		return false;
+	// 	}
+	// 	else {
+	// 		RCLCPP_INFO(nh->get_logger(), "Successfully unloaded nodelet %s", name.c_str());
+	// 		return true;
+	// 	}
+	// }
+	// else
+	// {
+	// 	RCLCPP_ERROR(nh->get_logger(), "Unable to unload nodelet %s: service unavailable", name.c_str());
+	// 	return false;
+	// }
 }
 
 } //namespace
diff --git a/src/dyknow_manager/src/configuration_node.cpp b/src/dyknow_manager/src/configuration_node.cpp
index 9d674c1..caa7b15 100644
--- a/src/dyknow_manager/src/configuration_node.cpp
+++ b/src/dyknow_manager/src/configuration_node.cpp
@@ -1,4 +1,4 @@
-#include "configuration_node.hpp"
+#include "dyknow_manager/configuration_node.hpp"
 
 namespace dyknow {
 
@@ -23,10 +23,10 @@ ConfigurationNode::ConfigurationNode(ConfigurationNode* parentPtr, Transformatio
 }
 
 ConfigurationNode::~ConfigurationNode() {
-	std::pair<int, std::vector<ConfigurationNode*> > pair;
-	BOOST_FOREACH(pair, optionMap) {
-		ConfigurationNode* node;
-		BOOST_FOREACH(node, pair.second) {
+	//std::pair<int, std::vector<ConfigurationNode*> > pair;
+	for(std::pair<int, std::vector<ConfigurationNode*> > pair : optionMap) {
+		//ConfigurationNode* node;
+		for (ConfigurationNode* node : pair.second) {
 			delete node;
 		}
 	}
diff --git a/src/dyknow_manager/src/configuration_planner.cpp b/src/dyknow_manager/src/configuration_planner.cpp
index 1f650ef..2cfcead 100644
--- a/src/dyknow_manager/src/configuration_planner.cpp
+++ b/src/dyknow_manager/src/configuration_planner.cpp
@@ -1,4 +1,4 @@
-#include "configuration_planner.hpp"
+#include "dyknow_manager/configuration_planner.hpp"
 
 namespace dyknow {
 
@@ -22,15 +22,15 @@ Change ConfigurationPlanner::optimise(std::vector<TargetSpec> targets, Computati
 
 	std::vector<TransformationSpec> targetTransforms;
 	std::vector<int> targetPorts;
-	TargetSpec target;
-	BOOST_FOREACH(target, targets) {
-		TransformationSpec spec;
-		BOOST_FOREACH(spec, graph.transformations) {
+	//TargetSpec target;
+	for (TargetSpec target : targets) {
+		//TransformationSpec spec;
+		for (TransformationSpec 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());
+				RCLCPP_INFO(nh->get_logger(), "Setting target: %s", concat(spec.label, "/"+(target.port+1)).c_str());
 				break;
 			}
 		}
@@ -48,19 +48,19 @@ Change ConfigurationPlanner::optimise(std::vector<TargetSpec> targets, Computati
 		// 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());
+		if(verbosity >= 9) RCLCPP_INFO(nh->get_logger(),"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(verbosity >= 3) RCLCPP_INFO(nh->get_logger(),"Entering depth: %i", depth);
+	while(expansionCount < MAX_EXPANSIONS && (nh->now() - startTime).seconds() < MAX_TIME_SEC && !queue.empty()) {
 		if(range == 0) {
-			depthExpansionTime.push_back(ros::Time::now()-startTime);
+			depthExpansionTime.push_back(nh->now()-startTime);
 			depthExpansionIndex.push_back(expansionCount-1);
 			depth++;
 			range = queue.size();
-			if(verbosity >= 3) ROS_INFO("Entering depth %i", depth);
+			if(verbosity >= 3) RCLCPP_INFO(nh->get_logger(),"Entering depth %i", depth);
 		}
 
 		ConfigurationNode* node = queue.front();
@@ -68,7 +68,7 @@ Change ConfigurationPlanner::optimise(std::vector<TargetSpec> targets, Computati
 		range--;
 
 		std::queue<ConfigurationNode*> expansion = expand(node, graph, matrix, verbosity);
-		if(verbosity >= 9) ROS_INFO("Adding %lu elements to the queue", expansion.size());
+		if(verbosity >= 9) RCLCPP_INFO(nh->get_logger(),"Adding %lu elements to the queue", expansion.size());
 		while(!expansion.empty()) {
 			queue.push(expansion.front());
 			expansion.pop();
@@ -76,94 +76,94 @@ Change ConfigurationPlanner::optimise(std::vector<TargetSpec> targets, Computati
 	}
 
 	// Performance reporting
-	if(verbosity >= 2) ROS_INFO("Done (%i expansions at %f sec)", expansionCount, (ros::Time::now()-startTime).toSec());
+	if(verbosity >= 2) RCLCPP_INFO(nh->get_logger(),"Done (%i expansions at %f sec)", expansionCount, (nh->now()-startTime).seconds());
 	if(verbosity >= 2) {
 		if(root->cost <= ConfigurationNode::MAX_COST) {
-			ROS_INFO("Found total solution: %f", root->cost);
-			ROS_INFO("---Score change---");
+			RCLCPP_INFO(nh->get_logger(),"Found total solution: %f", root->cost);
+			RCLCPP_INFO(nh->get_logger(),"---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());
+				RCLCPP_INFO(nh->get_logger(),"%i:\t%f\t [%f sec]", costIndex[index], costChange[index], costTime[index].seconds());
 			}
-			ROS_INFO("------------------");
+			RCLCPP_INFO(nh->get_logger(),"------------------");
 		}
 		else {
-			ROS_INFO("No total solution found");
+			RCLCPP_INFO(nh->get_logger(),"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);
+		RCLCPP_INFO(nh->get_logger(),"------Target scores------");
+		// std::pair<int, ConfigurationNode*> bestElem;
+		for (std::pair<int, ConfigurationNode*> bestElem : root->bestMap) {
+			RCLCPP_INFO(nh->get_logger(),"%i:\t%f",  bestElem.first+1, bestElem.second->cost);
 		}
-		ROS_INFO("----------------------------");
+		RCLCPP_INFO(nh->get_logger(),"----------------------------");
 
-		ROS_INFO("------Expansion depths------");
+		RCLCPP_INFO(nh->get_logger(),"------Expansion depths------");
 		for(int index = 0 ; index < depthExpansionIndex.size(); index++) {
-			ROS_INFO("%i:\t%i\t [%f sec]",  index, depthExpansionIndex[index], depthExpansionTime[index].toSec());
+			RCLCPP_INFO(nh->get_logger(),"%i:\t%i\t [%f sec]",  index, depthExpansionIndex[index], depthExpansionTime[index].seconds());
 		}
-		ROS_INFO("----------------------------");
+		RCLCPP_INFO(nh->get_logger(),"----------------------------");
 	}
 
 	root->print();
 
 	// Post-processing to remove duplicate subtrees
-	ros::Time postProcessStartTime = ros::Time::now();
+	rclcpp::Time postProcessStartTime = nh->now();
 	generateIndex(root, verbosity);
-	ros::Time postProcessEndTime = ros::Time::now();
+	rclcpp::Time postProcessEndTime = nh->now();
 
 	root->print();
 
 	// Convert resulting tree into delta
-	ros::Time changeGenStartTime = ros::Time::now();
+	rclcpp::Time changeGenStartTime = nh->now();
 	Change delta;
-	std::pair<int, ConfigurationNode*> bestElem;
-	BOOST_FOREACH(bestElem, root->bestMap) {
+	//std::pair<int, ConfigurationNode*> bestElem;
+	for (std::pair<int, ConfigurationNode*> bestElem : root->bestMap) {
 		generateChange(delta, bestElem.second, createUniqueName("stream"), graph, verbosity);
 	}
-	ros::Time changeGenEndTime = ros::Time::now();
+	rclcpp::Time changeGenEndTime = nh->now();
 
 	// Eliminate CUs if they are unused and unprotected
-	ros::Time gcStartTime = ros::Time::now();
+	rclcpp::Time gcStartTime = nh->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) RCLCPP_INFO(nh->get_logger(),"Constructed delta of size %i", delta.size());
+	rclcpp::Time gcEndTime = nh->now();
 
 	if(verbosity >= 2) {
-		ROS_INFO("---Method performance---");
-		ros::Duration expandTime;
+		RCLCPP_INFO(nh->get_logger(),"---Method performance---");
+		rclcpp::Duration expandTime = rclcpp::Duration::from_seconds(0.0);
 		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);
+		RCLCPP_INFO(nh->get_logger(),"Expansion:\t\t%f sec", expandTimes.size() > 0 ? expandTime.seconds() / (double)expandTimes.size() : 0.0);
 
-		ros::Duration expandCUTime;
+		rclcpp::Duration expandCUTime = rclcpp::Duration::from_seconds(0.0);
 		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);
+		RCLCPP_INFO(nh->get_logger(),"\tCU expansion:\t%f sec", expandCUTimes.size() > 0 ? expandCUTime.seconds() / (double)expandCUTimes.size() : 0.0);
 
-		ros::Duration expandTFTime;
+		rclcpp::Duration expandTFTime = rclcpp::Duration::from_seconds(0.0);
 		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);
+		RCLCPP_INFO(nh->get_logger(),"\tTF expansion:\t%f sec", expandTFTimes.size() > 0 ? expandTFTime.seconds() / (double)expandTFTimes.size() : 0.0);
 
-		ros::Duration updateTime;
+		rclcpp::Duration updateTime = rclcpp::Duration::from_seconds(0.0);
 		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);
+		RCLCPP_INFO(nh->get_logger(),"\tUpdate:\t\t%f sec", updateTimes.size() > 0 ? updateTime.seconds() / (double)updateTimes.size() : 0.0);
 
-		ros::Duration postProcessTime = postProcessEndTime - postProcessStartTime;
-		ROS_INFO("Post-process:\t\t%f sec", postProcessTime.toSec());
+		rclcpp::Duration postProcessTime = postProcessEndTime - postProcessStartTime;
+		RCLCPP_INFO(nh->get_logger(),"Post-process:\t\t%f sec", postProcessTime.seconds());
 
-		ros::Duration generateChangeTime = changeGenEndTime - changeGenStartTime;
-		ROS_INFO("Change generation:\t%f sec", generateChangeTime.toSec());
+		rclcpp::Duration generateChangeTime = changeGenEndTime - changeGenStartTime;
+		RCLCPP_INFO(nh->get_logger(),"Change generation:\t%f sec", generateChangeTime.seconds());
 
-		ros::Duration garbageCollectTime = gcEndTime - gcStartTime;
-		ROS_INFO("Garbage collection:\t%f sec", garbageCollectTime.toSec());
+		rclcpp::Duration garbageCollectTime = gcEndTime - gcStartTime;
+		RCLCPP_INFO(nh->get_logger(),"Garbage collection:\t%f sec", garbageCollectTime.seconds());
 
-		ROS_INFO("----------------------------");
+		RCLCPP_INFO(nh->get_logger(),"----------------------------");
 	}
 
 	// Clean and return result
@@ -176,7 +176,7 @@ Change ConfigurationPlanner::optimise(std::vector<TargetSpec> targets, Computati
 }
 
 std::queue<ConfigurationNode*> ConfigurationPlanner::expand(ConfigurationNode* nodePtr, ComputationGraph& graph, FeasibilityMatrix& matrix, unsigned int verbosity) {
-	ros::Time expandStart = ros::Time::now();
+	rclcpp::Time expandStart = nh->now();
 	std::queue<ConfigurationNode*> result;
 	int arity = nodePtr->type.inPorts.size();
 	bool update = false;
@@ -188,11 +188,11 @@ std::queue<ConfigurationNode*> ConfigurationPlanner::expand(ConfigurationNode* n
 	// 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();
+		rclcpp::Time expandCUStart = nh->now();
 		expandCompUnit(nodePtr, graph, matrix, result, i, ancestors, candidates, update, verbosity);
-		ros::Time expandTFStart = ros::Time::now();
+		rclcpp::Time expandTFStart = nh->now();
 		expandTransformation(nodePtr, matrix, result, i, ancestors, candidates, update, verbosity);
-		ros::Time expandFinish = ros::Time::now();
+		rclcpp::Time expandFinish = nh->now();
 
 		expandCUTimes.push_back(expandTFStart - expandCUStart);
 		expandTFTimes.push_back(expandFinish - expandTFStart);
@@ -200,15 +200,15 @@ std::queue<ConfigurationNode*> ConfigurationPlanner::expand(ConfigurationNode* n
 
 	// Only update when needed
 	if(update) {
-		ros::Time updateStart = ros::Time::now();
+		rclcpp::Time updateStart = nh->now();
 		this->update(nodePtr, matrix);
-		ros::Time updateFinish = ros::Time::now();
+		rclcpp::Time updateFinish = nh->now();
 
 		updateTimes.push_back(updateFinish - updateStart);
 	}
 
-	if(verbosity >= 9) ROS_INFO("Expansion count: %i", expansionCount);
-	ros::Time expandFinish = ros::Time::now();
+	if(verbosity >= 9) RCLCPP_INFO(nh->get_logger(),"Expansion count: %i", expansionCount);
+	rclcpp::Time expandFinish = nh->now();
 	expandTimes.push_back(expandFinish - expandStart); //TODO: Move timers
 	return result;
 }
@@ -219,17 +219,17 @@ void ConfigurationPlanner::expandCompUnit(ConfigurationNode* nodePtr, Computatio
 	// Consider using computation units
 	double bestCost = nodePtr->hasBest(index) ? nodePtr->bestMap[index]->cost : ConfigurationNode::MAX_COST + 1;
 
-	Port candidate;
-	BOOST_FOREACH(candidate, candidates) {
+	// Port candidate;
+	for (Port 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());
+//		if(verbosity >= 9) RCLCPP_INFO(nh->get_logger(),"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) {
+		for (ComputationUnit 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); }
+				if(verbosity >= 9) { RCLCPP_INFO(nh->get_logger(),"Found matching computation unit %s for transformation %s with port /%i", compUnit.name.c_str(), candidateSpec.label.c_str(), candidate.second+1); }
 
 
 				TransformationSpec spec;
@@ -247,7 +247,7 @@ void ConfigurationPlanner::expandCompUnit(ConfigurationNode* nodePtr, Computatio
 					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(verbosity >= 9) RCLCPP_INFO(nh->get_logger(),"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);
 					}
@@ -270,13 +270,13 @@ void ConfigurationPlanner::expandTransformation(ConfigurationNode* nodePtr, Feas
 	// Consider using a new instance
 	double bestCost = nodePtr->hasBest(index) ? nodePtr->bestMap[index]->cost : ConfigurationNode::MAX_COST + 1;
 
-	Port candidate;
-	BOOST_FOREACH(candidate, candidates) {
+	//Port candidate;
+	for (Port 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(verbosity >= 9) RCLCPP_INFO(nh->get_logger(),"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());
+			if(verbosity >= 3) RCLCPP_INFO(nh->get_logger(),"May not reuse: %s (skipping)", candidateSpec.label.c_str());
 			continue;
 		}
 
@@ -288,7 +288,7 @@ void ConfigurationPlanner::expandTransformation(ConfigurationNode* nodePtr, Feas
 			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(verbosity >= 9) RCLCPP_INFO(nh->get_logger(),"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);
 			}
@@ -310,16 +310,16 @@ void ConfigurationPlanner::update(ConfigurationNode* nodePtr, FeasibilityMatrix&
 	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) {
+		//ConfigurationNode* option;
+		for (ConfigurationNode* 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);
+				if(verbosity >= 9) RCLCPP_INFO(nh->get_logger(),"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);
+				if(verbosity >= 9) RCLCPP_INFO(nh->get_logger(),"Old best (%s): %f is not better than %f", option->type.label.c_str(), newCost, best);
 			}
 		}
 
@@ -331,7 +331,7 @@ void ConfigurationPlanner::update(ConfigurationNode* nodePtr, FeasibilityMatrix&
 			cost = ConfigurationNode::MAX_COST+1;
 		}
 	}
-	if(verbosity >= 9) ROS_INFO("New cost (%s): %f", nodePtr->type.label.c_str(), cost);
+	if(verbosity >= 9) RCLCPP_INFO(nh->get_logger(),"New cost (%s): %f", nodePtr->type.label.c_str(), cost);
 
 	if(cascade) {
 		nodePtr->cost = cost;
@@ -339,11 +339,11 @@ void ConfigurationPlanner::update(ConfigurationNode* nodePtr, FeasibilityMatrix&
 	}
 
 	if(nodePtr->parentPtr == NULL && prevBest != nodePtr->cost) {
-		if(verbosity >= 9) ROS_INFO("***** Found solution with cost %f *****", nodePtr->cost);
+		if(verbosity >= 9) RCLCPP_INFO(nh->get_logger(),"***** 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);
+		costTime.push_back(nh->now() - startTime);
 	}
 }
 
@@ -351,7 +351,7 @@ void ConfigurationPlanner::generateChange(Change& change, ConfigurationNode* nod
 
 	// Futures are placeholders that cannot be configured
 	if(nodePtr->isFuture()) {
-		if(verbosity >= 9) ROS_INFO("Skipping future %s", nodePtr->type.source.c_str() );
+		if(verbosity >= 9) RCLCPP_INFO(nh->get_logger(),"Skipping future %s", nodePtr->type.source.c_str() );
 		return;
 	}
 
@@ -388,7 +388,7 @@ void ConfigurationPlanner::generateChange(Change& change, ConfigurationNode* nod
 	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());
+		if(verbosity >= 9) RCLCPP_INFO(nh->get_logger(),"Setting future: %s => %s", concat(key, outPort).c_str(), outStream.c_str());
 		out[outPort] = outStream;
 	}
 
@@ -397,7 +397,7 @@ void ConfigurationPlanner::generateChange(Change& change, ConfigurationNode* nod
 	std::map<std::string, std::string> in;
 	if(!futureNode) {
 		std::pair<int, ConfigurationNode*> bestElem;
-		BOOST_FOREACH(bestElem, nodePtr->bestMap) {
+		for(std::pair<int, ConfigurationNode*> 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
@@ -421,15 +421,15 @@ void ConfigurationPlanner::generateChange(Change& change, ConfigurationNode* nod
 //					}
 //					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 >= 9) RCLCPP_INFO(nh->get_logger(),"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());
+					if(verbosity >= 3) RCLCPP_INFO(nh->get_logger(),"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]) {
+					for ( std::pair<std::string, std::string> pair : graph.generatorMap[cuName]) {
 						std::string bestOutPort = "/" + to_string(bestElem.second->outPort+1);
 						if(pair.first.compare(bestOutPort) == 0) {
 							streamName = pair.second;
@@ -451,17 +451,17 @@ void ConfigurationPlanner::generateChange(Change& change, ConfigurationNode* nod
 	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());
+		// std::pair<std::string, std::string> pair;
+		for (std::pair<std::string, std::string> pair : in) {
+			RCLCPP_INFO(nh->get_logger(),"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());
+		for (std::pair<std::string, std::string> pair : out) {
+			RCLCPP_INFO(nh->get_logger(),"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());
+		if(verbosity >= 3) RCLCPP_INFO(nh->get_logger(),"Protecting CU %s", name.c_str());
 		protections.push_back(name);
 	}
 }
@@ -470,7 +470,7 @@ void ConfigurationPlanner::garbageCollect(Change& change, ComputationGraph& grap
 	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());
+			if(verbosity >= 3) RCLCPP_INFO(nh->get_logger(),"Marking CU %s for unloading", graph.computationUnits[i].name.c_str());
 
 			DestroyCommand* destroyCmdPtr = new DestroyCommand(graph.computationUnits[i].name);
 			change.add(destroyCmdPtr);
@@ -480,11 +480,11 @@ void ConfigurationPlanner::garbageCollect(Change& change, ComputationGraph& grap
 
 void ConfigurationPlanner::generateIndex(ConfigurationNode* nodePtr, unsigned int verbosity) {
 
-	std::pair<int, ConfigurationNode*> bestCandidate;
-	BOOST_FOREACH(bestCandidate, nodePtr->bestMap) {
+	// std::pair<int, ConfigurationNode*> bestCandidate;
+	for (std::pair<int, ConfigurationNode*> 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);
+			RCLCPP_WARN(nh->get_logger(), "No solution for target %s at root port %i; skipping", bestCandidate.second->type.label.c_str(), bestCandidate.first);
 			continue;
 		}
 
@@ -496,7 +496,7 @@ void ConfigurationPlanner::generateIndex(ConfigurationNode* nodePtr, unsigned in
 		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());
+			if(verbosity >=9) RCLCPP_INFO(nh->get_logger(),"Guarding %s for duplication", portLabel.c_str());
 
 			// Recursively apply subtree removal
 			generateIndex(bestCandidate.second, verbosity);
@@ -504,7 +504,7 @@ void ConfigurationPlanner::generateIndex(ConfigurationNode* nodePtr, unsigned in
 		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());
+			if(verbosity >= 3) RCLCPP_INFO(nh->get_logger(),"Swapping duplicate tree with root %s for future", portLabel.c_str());
 //			int outPort = bestCandidate.second->outPort;
 			TransformationSpec spec = bestCandidate.second->type;
 			spec.type = COMPUNIT;
@@ -533,7 +533,7 @@ std::string ConfigurationPlanner::createUniqueName(std::string base) {
 
 	// Empty base strings are not allowed
 	if(base == "") {
-		ROS_WARN("Empty base strings are not allowed for unique names");
+		RCLCPP_WARN(nh->get_logger(), "Empty base strings are not allowed for unique names");
 		base = "reserved";
 	}
 
@@ -543,7 +543,8 @@ std::string ConfigurationPlanner::createUniqueName(std::string base) {
 	}
 
 	// Append label with dyknow_ and prepend it with the unique identifier
-	std::string suffix = boost::lexical_cast<std::string>((int)uniqueNameMap[base]);
+	//std::string suffix = boost::lexical_cast<std::string>((int)uniqueNameMap[base]);
+	std::string suffix = std::to_string(static_cast<int>(uniqueNameMap[base]));
 	uniqueNameMap[base]++;
 	return ("/dyknow/" + base + "_" + suffix);
 }
diff --git a/src/dyknow_manager/src/feasibility_matrix.cpp b/src/dyknow_manager/src/feasibility_matrix.cpp
index 83fec11..3ebaac6 100644
--- a/src/dyknow_manager/src/feasibility_matrix.cpp
+++ b/src/dyknow_manager/src/feasibility_matrix.cpp
@@ -1,4 +1,4 @@
-#include "feasibility_matrix.hpp"
+#include "dyknow_manager/feasibility_matrix.hpp"
 
 namespace dyknow {
 
@@ -14,13 +14,13 @@ std::vector<Port> FeasibilityMatrix::getValid(Port input) {
 
 TransformationSpec FeasibilityMatrix::decode(Port port) {
 	TransformationSpec spec;
-	BOOST_FOREACH(spec, library) {
+	for (TransformationSpec spec : library) {
 		if(spec.id == port.first) {
 			return spec;
 		}
 	}
 
-	ROS_WARN("Unable to decode port (%i, %i)", port.first, port.second);
+	//RCLCPP_WARN(nh->get_logger(), "Unable to decode port (%i, %i)", port.first, port.second);
 	return spec;
 }
 
@@ -33,8 +33,8 @@ void FeasibilityMatrix::invalidate(std::string transformation) {
 			return;
 		}
 	}
-
-	ROS_WARN("Unknown transformation %s marked for invalidation; ignoring", transformation.c_str());
+	// nh is not avaliable here =(
+	//RCLCPP_WARN(nh->get_logger(), "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 297db62..18357f1 100644
--- a/src/dyknow_manager/src/manager_componet.cpp
+++ b/src/dyknow_manager/src/manager_componet.cpp
@@ -6,14 +6,14 @@ namespace manager {
 	ManagerComponet::ManagerComponet(const rclcpp::NodeOptions & options): 
 		node(std::make_shared<dyknow::Node>("manager_componet", options)) {
 
-			std::shared_ptr<dyknow::ConfigurationManager> configManPtr = std::shared_ptr<dyknow::ConfigurationManager>(new dyknow::ConfigurationManager(node));
+			RCLCPP_INFO(node->get_logger(), "I LIVE, AM I ALIVE, I AM MEEEEE ");
+			configManPtr = std::shared_ptr<dyknow::ConfigurationManager>(new dyknow::ConfigurationManager(this->node));
 		}
 	
 	rclcpp::node_interfaces::NodeBaseInterface::SharedPtr ManagerComponet::get_node_base_interface() const
     {
     return this->node->get_node_base_interface();
-    }
-	
+    }	
 }
 
 RCLCPP_COMPONENTS_REGISTER_NODE(manager::ManagerComponet)
\ No newline at end of file
diff --git a/src/dyknow_manager/src/target_spec.cpp b/src/dyknow_manager/src/target_spec.cpp
index 7924737..7f96fad 100644
--- a/src/dyknow_manager/src/target_spec.cpp
+++ b/src/dyknow_manager/src/target_spec.cpp
@@ -1,12 +1,12 @@
-#include "target_spec.hpp"
+#include "dyknow_manager/target_spec.hpp"
 
 namespace dyknow {
 
-TargetSpec TargetFactory::create(int id, std::string label, int port, std::string topic) {
+TargetSpec 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 create(dyknow_interfaces::msg::Target target) {
 	TargetSpec spec;
 	spec.label = target.label;
 	spec.port = target.port;
-- 
GitLab