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