From 0828bd42e97d9135ce98cd7ae0e7acdeab703c2b Mon Sep 17 00:00:00 2001
From: leolol <leoja464@student.liu.se>
Date: Mon, 27 Jan 2025 13:05:10 +0000
Subject: [PATCH] fun seg fault
---
src/dyknow_echo/CMakeLists.txt | 20 +-
src/dyknow_echo/src/echo.cpp | 3 +-
src/dyknow_echo/src/echoComposition.cpp | 56 +++-
src/dyknow_manager/CMakeLists.txt | 25 +-
.../dyknow_manager/compunit_helper.hpp | 2 +
.../dyknow_manager/configuration_manager.hpp | 13 +-
src/dyknow_manager/src/change.cpp | 2 +-
src/dyknow_manager/src/compunit_helper.cpp | 54 ++--
.../src/configuration_manager.cpp | 276 +++++++++---------
src/dyknow_manager/src/environment.cpp | 26 +-
src/dyknow_manager/src/manager_componet.cpp | 8 +-
.../src/transformation_spec.cpp | 4 +-
.../include/dyknow_nodehandle/node.hpp | 3 -
13 files changed, 275 insertions(+), 217 deletions(-)
diff --git a/src/dyknow_echo/CMakeLists.txt b/src/dyknow_echo/CMakeLists.txt
index 9b2a3c0..25d31a6 100644
--- a/src/dyknow_echo/CMakeLists.txt
+++ b/src/dyknow_echo/CMakeLists.txt
@@ -11,6 +11,7 @@ find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(dyknow_nodehandle REQUIRED)
find_package(dyknow_analytics REQUIRED)
+find_package(composition_interfaces REQUIRED)
# include_directories(
# include
@@ -20,8 +21,8 @@ find_package(dyknow_analytics REQUIRED)
# Declare a shared library
add_library(dyknow_echo_component SHARED
- src/echo.cpp
- #src/echoComposition.cpp # Change to the composition version
+ #src/echo.cpp
+ src/echoComposition.cpp # Change to the composition version
)
target_include_directories(dyknow_echo_component
@@ -35,18 +36,19 @@ ament_target_dependencies(dyknow_echo_component
rclcpp_components
dyknow_nodehandle
dyknow_analytics
+ composition_interfaces
)
+# rclcpp_components_register_node(dyknow_echo_component
+# PLUGIN "echo::Echo"
+# EXECUTABLE dyknow_echo
+# )
+
rclcpp_components_register_node(dyknow_echo_component
- PLUGIN "echo::Echo"
- EXECUTABLE dyknow_echo
+ PLUGIN "echo::EchoComp"
+ EXECUTABLE dyknow_echo
)
-#rclcpp_components_register_node(dyknow_echo_component
-# PLUGIN "echo::EchoComp"
-# EXECUTABLE dyknow_echo
-#)
-
install(TARGETS dyknow_echo_component
EXPORT export_dyknow_echo_component
diff --git a/src/dyknow_echo/src/echo.cpp b/src/dyknow_echo/src/echo.cpp
index 331d54f..e181239 100644
--- a/src/dyknow_echo/src/echo.cpp
+++ b/src/dyknow_echo/src/echo.cpp
@@ -21,8 +21,7 @@ namespace echo {
//copy.header = sample->header;
//copy.valid_time = sample->valid_time;
copy.test = sample->test;
- //RCLCPP_INFO(this->get_logger(), "I LIVE, AM I ALIVE, I AM MEEEEE ");
- RCLCPP_INFO(this->get_logger(), this->getOutputs()[0].c_str());
+ RCLCPP_INFO(this->get_logger(), "I LIVE, AM I ALIVE, I AM MEEEEE ");
pub.publish(copy);
}
diff --git a/src/dyknow_echo/src/echoComposition.cpp b/src/dyknow_echo/src/echoComposition.cpp
index 4bce894..5af9000 100644
--- a/src/dyknow_echo/src/echoComposition.cpp
+++ b/src/dyknow_echo/src/echoComposition.cpp
@@ -1,5 +1,6 @@
#include "dyknow_echo/echoComposition.hpp"
#include <rclcpp_components/register_node_macro.hpp>
+#include <composition_interfaces/srv/load_node.hpp>
namespace echo {
@@ -8,26 +9,63 @@ namespace echo {
{
int arity = 10;
for(int i = 0; i < arity; i++) {
- node->create_subscription<dyknow_nodehandle::msg::Sample, EchoComp>(1000, &EchoComp::callback, this);
+ node->create_subscription<dyknow_interfaces::msg::Sample, EchoComp>(1000, &EchoComp::callback, this);
}
node->connectInput("_1", "in");
- pub = node->create_publisher<dyknow_nodehandle::msg::Sample>(1000);
+ pub = node->create_publisher<dyknow_interfaces::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;
+ void EchoComp::callback(std::shared_ptr<dyknow_interfaces::msg::Sample>& sample) {
+ dyknow_interfaces::msg::Sample copy;
+ //copy.fields = sample->fields;
+ //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
+ {
+ RCLCPP_ERROR(node->get_logger(), "Service %s/load_nodelet not available");
+ return;
+ }
+ RCLCPP_INFO(node->get_logger(), "");
+
+
+ // 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;
+ }
+
pub.publish(copy);
}
diff --git a/src/dyknow_manager/CMakeLists.txt b/src/dyknow_manager/CMakeLists.txt
index 57b94d2..91df589 100644
--- a/src/dyknow_manager/CMakeLists.txt
+++ b/src/dyknow_manager/CMakeLists.txt
@@ -14,27 +14,28 @@ find_package(dyknow_nodehandle REQUIRED)
find_package(dyknow_analytics REQUIRED)
find_package(rosidl_default_generators REQUIRED)
-# include_directories( # Testing if this part is needed or not....
-# include
-# ${rclcpp_INCLUDE_DIRS}
-# ${dyknow_nodehandle_INCLUDE_DIRS}
-# ${dyknow_analytics_INCLUDE_DIRS}
-# )
-
# Define our target, should this not be all of our cpp files?
add_library(${PROJECT_NAME} SHARED
src/manager_componet.cpp
- #src/configuration_manager.cpp # if this is included the project fails to compile
+ src/configuration_manager.cpp
+ src/transformation_spec.cpp
+ src/environment.cpp
+ src/change.cpp
+ src/compunit_helper.cpp
+
+
) # this should be all the source files that are requierd for compiling the lib
-target_include_directories(${PROJECT_NAME} PUBLIC
+target_include_directories(${PROJECT_NAME}
+ PUBLIC
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include>"
)
# Link dependencies to our target ,hmmm
# For this we might instead conider using target_link_libraries, works the same but with namespaces
-ament_target_dependencies(${PROJECT_NAME}
+ament_target_dependencies(
+ ${PROJECT_NAME}
rclcpp
rclcpp_components
dyknow_nodehandle
@@ -45,6 +46,7 @@ rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN "manager::ManagerComponet"
EXECUTABLE dyknow_manager_componet
)
+
install(
TARGETS ${PROJECT_NAME}
EXPORT export_${PROJECT_NAME}
@@ -60,13 +62,10 @@ install(
)
# This will tell any packages downstream what dependencies that this library has to we dont need to call add_packages for them too
-#ament_export_dependencies(some_dependency)
-
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_export_dependencies(dyknow_nodehandle)
ament_export_include_directories(include)
-
ament_package()
diff --git a/src/dyknow_manager/include/dyknow_manager/compunit_helper.hpp b/src/dyknow_manager/include/dyknow_manager/compunit_helper.hpp
index 9ca5182..7579bf0 100644
--- a/src/dyknow_manager/include/dyknow_manager/compunit_helper.hpp
+++ b/src/dyknow_manager/include/dyknow_manager/compunit_helper.hpp
@@ -13,6 +13,8 @@
#include "dyknow_nodehandle/node.hpp"
#include "transformation_spec.hpp"
+#include <composition_interfaces/srv/load_node.hpp>
+
namespace dyknow {
diff --git a/src/dyknow_manager/include/dyknow_manager/configuration_manager.hpp b/src/dyknow_manager/include/dyknow_manager/configuration_manager.hpp
index 2fd8fd6..3edfd37 100644
--- a/src/dyknow_manager/include/dyknow_manager/configuration_manager.hpp
+++ b/src/dyknow_manager/include/dyknow_manager/configuration_manager.hpp
@@ -23,7 +23,7 @@
#include "dyknow_interfaces/srv/exp_populate.hpp"
#include "dyknow_interfaces/msg/unit.hpp"
#include "dyknow_interfaces/msg/transformation.hpp"
-#include "dyknow_interfaces/srv/configure.hpp"
+#include "dyknow_interfaces/srv/manager_configure.hpp"
#include "dyknow_interfaces/msg/connection.hpp"
#include "dyknow_interfaces/msg/target.hpp"
#include "dyknow_interfaces/srv/get_valid_inputs.hpp"
@@ -45,7 +45,7 @@ public:
, removeTransformationService(nh->create_service<dyknow_interfaces::srv::RemoveTransformation>("remove_transformation", std::bind(&ConfigurationManager::removeTransformationCallback, this, std::placeholders::_1, std::placeholders::_2)))
, spawnService(nh->create_service<dyknow_interfaces::srv::Spawn>("spawn", std::bind(&ConfigurationManager::spawnCallback, this, std::placeholders::_1, std::placeholders::_2)))
, destroyService(nh->create_service<dyknow_interfaces::srv::Destroy>("destroy", std::bind(&ConfigurationManager::destroyCallback, this, std::placeholders::_1, std::placeholders::_2)))
- , configureService(nh->create_service<dyknow_interfaces::srv::Configure>("configure", std::bind(&ConfigurationManager::configureCallback, this, std::placeholders::_1, std::placeholders::_2)))
+ , configureService(nh->create_service<dyknow_interfaces::srv::ManagerConfigure>("configure", std::bind(&ConfigurationManager::configureCallback, this, std::placeholders::_1, std::placeholders::_2)))
, addTargetService(nh->create_service<dyknow_interfaces::srv::AddTarget>("add_target", std::bind(&ConfigurationManager::addTargetCallback, this, std::placeholders::_1, std::placeholders::_2)))
, removeTargetService(nh->create_service<dyknow_interfaces::srv::RemoveTarget>("remove_target", std::bind(&ConfigurationManager::removeTargetCallback, this, std::placeholders::_1, std::placeholders::_2)))
, refreshService(nh->create_service<dyknow_interfaces::srv::Refresh>("refresh", std::bind(&ConfigurationManager::refreshCallback, this, std::placeholders::_1, std::placeholders::_2)))
@@ -71,7 +71,7 @@ public:
void removeTransformationCallback(const std::shared_ptr<dyknow_interfaces::srv::RemoveTransformation::Request> req, std::shared_ptr<dyknow_interfaces::srv::RemoveTransformation::Response> res);
void spawnCallback(const std::shared_ptr<dyknow_interfaces::srv::Spawn::Request> req, std::shared_ptr<dyknow_interfaces::srv::Spawn::Response> res);
void destroyCallback(const std::shared_ptr<dyknow_interfaces::srv::Destroy::Request> req, std::shared_ptr<dyknow_interfaces::srv::Destroy::Response> res);
- void configureCallback(const std::shared_ptr<dyknow_interfaces::srv::Configure::Request> req, std::shared_ptr<dyknow_interfaces::srv::Configure::Response> res);
+ void configureCallback(const std::shared_ptr<dyknow_interfaces::srv::ManagerConfigure::Request> req, std::shared_ptr<dyknow_interfaces::srv::ManagerConfigure::Response> res);
void addTargetCallback(const std::shared_ptr<dyknow_interfaces::srv::AddTarget::Request> req, std::shared_ptr<dyknow_interfaces::srv::AddTarget::Response> res);
void removeTargetCallback(const std::shared_ptr<dyknow_interfaces::srv::RemoveTarget::Request> req, std::shared_ptr<dyknow_interfaces::srv::RemoveTarget::Response> res);
void refreshCallback(const std::shared_ptr<dyknow_interfaces::srv::Refresh::Request> req, std::shared_ptr<dyknow_interfaces::srv::Refresh::Response> res);
@@ -103,7 +103,7 @@ private:
rclcpp::Service<dyknow_interfaces::srv::RemoveTransformation>::SharedPtr removeTransformationService;
rclcpp::Service<dyknow_interfaces::srv::Spawn>::SharedPtr spawnService;
rclcpp::Service<dyknow_interfaces::srv::Destroy>::SharedPtr destroyService;
- rclcpp::Service<dyknow_interfaces::srv::Configure>::SharedPtr configureService;
+ rclcpp::Service<dyknow_interfaces::srv::ManagerConfigure>::SharedPtr configureService;
rclcpp::Service<dyknow_interfaces::srv::AddTarget>::SharedPtr addTargetService;
rclcpp::Service<dyknow_interfaces::srv::RemoveTarget>::SharedPtr removeTargetService;
rclcpp::Service<dyknow_interfaces::srv::Refresh>::SharedPtr refreshService;
@@ -114,9 +114,6 @@ private:
std::mutex envAccess;
};
-
} //namespace
-
-
-#endif /* INCLUDE_CONFIGURATION_MANAGER_HPP_ */
+#endif /* INCLUDE_CONFIGURATION_MANAGER_HPP_ */
\ No newline at end of file
diff --git a/src/dyknow_manager/src/change.cpp b/src/dyknow_manager/src/change.cpp
index 75c19ab..4b05fab 100644
--- a/src/dyknow_manager/src/change.cpp
+++ b/src/dyknow_manager/src/change.cpp
@@ -1,4 +1,4 @@
-#include "change.hpp"
+#include "dyknow_manager/change.hpp"
namespace dyknow {
diff --git a/src/dyknow_manager/src/compunit_helper.cpp b/src/dyknow_manager/src/compunit_helper.cpp
index faeeb9b..11bf653 100644
--- a/src/dyknow_manager/src/compunit_helper.cpp
+++ b/src/dyknow_manager/src/compunit_helper.cpp
@@ -1,66 +1,74 @@
-#include "compunit_helper.hpp"
+#include "dyknow_manager/compunit_helper.hpp"
namespace dyknow {
-bool CompUnitHelper::spawn(dyknow::NodeHandle& nh, const std::string& manager, const std::string& name, const std::string& source, std::vector<std::string>& args) {
+bool CompUnitHelper::spawn(std::shared_ptr<dyknow::Node> nh, const std::string& manager, const std::string& name, const std::string& source, std::vector<std::string>& args) {
if(source == "") {
- ROS_WARN("Failed to spawn: %s does not provide a source", name.c_str());
+ RCLCPP_WARN(nh->get_logger(), "Failed to spawn: %s does not provide a source", name.c_str());
return false;
}
- ros::ServiceClient client = nh.serviceClient<nodelet::NodeletLoad>(manager + "/load_nodelet");
- nodelet::NodeletLoad loadNodeletService;
- loadNodeletService.request.name = name;
- loadNodeletService.request.type = source;
- loadNodeletService.request.my_argv = args;
+
+ // 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))
{
- ROS_INFO("Successfully spawned nodelet %s", name.c_str());
+ RCLCPP_INFO(nh->get_logger(), "Successfully spawned nodelet %s", name.c_str());
return true;
}
else
{
- ROS_ERROR("Unable to call service for %s with source %s", name.c_str(), source.c_str());
+ RCLCPP_ERROR(nh->get_logger(), "Unable to call service for %s with source %s", name.c_str(), source.c_str());
return false;
}
}
-bool CompUnitHelper::configure(dyknow::NodeHandle& nh, const std::string& manager, std::string name, std::map<std::string, std::string> inputMap, std::map<std::string, std::string> outputMap) {
+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;
- BOOST_FOREACH(elem, outputMap) {
+ //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);
- }
+ });
- BOOST_FOREACH(elem, inputMap) {
+ 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) {
- ROS_ERROR("Failed to configure nodelet %s", name.c_str());
+ RCLCPP_ERROR(nh->get_logger(), "Failed to configure nodelet %s", name.c_str());
}
else {
- ROS_INFO("Successfully configured nodelet %s", name.c_str());
+ RCLCPP_INFO(nh->get_logger(), "Successfully configured nodelet %s", name.c_str());
}
return true;
}
else
{
- ROS_ERROR("Unable to configure nodelet %s", name.c_str());
+ RCLCPP_ERROR(nh->get_logger(), "Unable to configure nodelet %s", name.c_str());
return false;
}
}
-bool CompUnitHelper::destroy(dyknow::NodeHandle& nh, const std::string& manager, const std::string& name) {
+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;
@@ -70,17 +78,17 @@ bool CompUnitHelper::destroy(dyknow::NodeHandle& nh, const std::string& manager,
if (unloadClient.call(unloadService))
{
if(!unloadService.response.success) {
- ROS_ERROR("Unable to unload nodelet %s: failure while unloading", name.c_str());
+ RCLCPP_ERROR(nh->get_logger(), "Unable to unload nodelet %s: failure while unloading", name.c_str());
return false;
}
else {
- ROS_INFO("Successfully unloaded nodelet %s", name.c_str());
+ RCLCPP_INFO(nh->get_logger(), "Successfully unloaded nodelet %s", name.c_str());
return true;
}
}
else
{
- ROS_ERROR("Unable to unload nodelet %s: service unavailable", name.c_str());
+ RCLCPP_ERROR(nh->get_logger(), "Unable to unload nodelet %s: service unavailable", name.c_str());
return false;
}
}
diff --git a/src/dyknow_manager/src/configuration_manager.cpp b/src/dyknow_manager/src/configuration_manager.cpp
index 4b7add1..da816b3 100644
--- a/src/dyknow_manager/src/configuration_manager.cpp
+++ b/src/dyknow_manager/src/configuration_manager.cpp
@@ -1,4 +1,4 @@
-#include "configuration_manager.hpp"
+#include "dyknow_manager/configuration_manager.hpp"
namespace dyknow {
@@ -8,19 +8,19 @@ void ConfigurationManager::createEnvironment(int id, std::string manager) {
environments.push_back(env);
}
-void ConfigurationManager::getConfigurationCallback(const std::shared_ptr<dyknow_manager::GetConfiguration::Request> req, std::shared_ptr<dyknow_manager::GetConfiguration::Response> res) {
+void ConfigurationManager::getConfigurationCallback(const std::shared_ptr<dyknow_interfaces::srv::GetConfiguration::Request> req, std::shared_ptr<dyknow_interfaces::srv::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;
+ dyknow_interfaces::msg::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;
+ dyknow_interfaces::msg::Connection con;
con.port = pair1.first;
con.topic = pair1.second;
unit.incoming.push_back(con);
@@ -28,18 +28,18 @@ void ConfigurationManager::getConfigurationCallback(const std::shared_ptr<dyknow
std::map<std::string, Generator> generatorMap = env.getGenerators();
for (auto& pair2 : generatorMap[cu.name]) {
- dyknow_manager::Connection con;
+ dyknow_interfaces::msg::Connection con;
con.port = pair2.first;
con.topic = pair2.second;
unit.outgoing.push_back(con);
}
- res.units.push_back(unit);
+ res->units.push_back(unit);
}
std::vector<TransformationSpec> transformations = env.getTransformations();
for (auto& spec : transformations) {
- dyknow_manager::Transformation tf;
+ dyknow_interfaces::msg::Transformation tf;
tf.label = spec.label;
tf.source = spec.source;
tf.in_ports = spec.inPorts;
@@ -48,231 +48,244 @@ void ConfigurationManager::getConfigurationCallback(const std::shared_ptr<dyknow
//TODO: Type, tag and params
tf.environment = env.getId();
- res.transformations.push_back(tf);
+ res->transformations.push_back(tf);
}
std::vector<TargetSpec> targets = env.getTargets();
for (auto& targetSpec : targets) {
- dyknow_manager::Target tgt;
+ dyknow_interfaces::msg::Target tgt;
tgt.label = targetSpec.label;
tgt.port = targetSpec.port;
tgt.topic = targetSpec.topic;
tgt.environment = env.getId();
- res.targets.push_back(tgt);
+ res->targets.push_back(tgt);
}
}
return;
}
-void ConfigurationManager::addTransformationCallback(const std::shared_ptr<dyknow_manager::AddTransformation::Request> req, std::shared_ptr<dyknow_manager::AddTransformation::Response> res) {
+void ConfigurationManager::addTransformationCallback(const std::shared_ptr<dyknow_interfaces::srv::AddTransformation::Request> req, std::shared_ptr<dyknow_interfaces::srv::AddTransformation::Response> res) {
Change delta;
- for (auto& tfSpec : req.transformations) {
+ 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;
+ enqueue(std::pair<int, Change>(req->envid, delta));
+ res->success = true;
return;
}
-void ConfigurationManager::removeTransformationCallback(const std::shared_ptr<dyknow_manager::RemoveTransformation::Request> req, std::shared_ptr<dyknow_manager::RemoveTransformation::Response> res) {
+void ConfigurationManager::removeTransformationCallback(const std::shared_ptr<dyknow_interfaces::srv::RemoveTransformation::Request> req, std::shared_ptr<dyknow_interfaces::srv::RemoveTransformation::Response> res) {
Change delta;
- for (auto& label : req.transformations) {
+ for (auto& label : req->transformations) {
RemoveCommand* cmdPtr = new RemoveCommand(label);
delta.add(cmdPtr);
}
- enqueue(std::pair<int, Change>(req.envId, delta));
- res.success = true;
+ enqueue(std::pair<int, Change>(req->envid, delta));
+ res->success = true;
return;
}
-void ConfigurationManager::spawnCallback(const std::shared_ptr<dyknow_manager::Spawn::Request> req, std::shared_ptr<dyknow_manager::Spawn::Response> res) {
+void ConfigurationManager::spawnCallback(const std::shared_ptr<dyknow_interfaces::srv::Spawn::Request> req, std::shared_ptr<dyknow_interfaces::srv::Spawn::Response> res) {
Change delta;
- SpawnCommand* cmdPtr = new SpawnCommand(req.name, req.type, req.protect);
+ SpawnCommand* cmdPtr = new SpawnCommand(req->name, req->type, req->protect);
delta.add(cmdPtr);
- enqueue(std::pair<int, Change>(req.envId, delta));
- res.success = true;
+ enqueue(std::pair<int, Change>(req->envid, delta));
+ res->success = true;
return;
}
-void ConfigurationManager::destroyCallback(const std::shared_ptr<dyknow_manager::Destroy::Request> req, std::shared_ptr<dyknow_manager::Destroy::Response> res) {
+void ConfigurationManager::destroyCallback(const std::shared_ptr<dyknow_interfaces::srv::Destroy::Request> req, std::shared_ptr<dyknow_interfaces::srv::Destroy::Response> res) {
Change delta;
- DestroyCommand* cmdPtr = new DestroyCommand(req.name);
+ DestroyCommand* cmdPtr = new DestroyCommand(req->name);
delta.add(cmdPtr);
- enqueue(std::pair<int, Change>(req.envId, delta));
- res.success = true;
+ enqueue(std::pair<int, Change>(req->envid, delta));
+ res->success = true;
return;
}
-void ConfigurationManager::configureCallback(const std::shared_ptr<dyknow_manager::Configure::Request> req, std::shared_ptr<dyknow_manager::Configure::Response> res) {
+void ConfigurationManager::configureCallback(const std::shared_ptr<dyknow_interfaces::srv::ManagerConfigure::Request> req, std::shared_ptr<dyknow_interfaces::srv::ManagerConfigure::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;
+ 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;
}
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]);
+ 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]);
+ 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);
+ ConfigureCommand* cmdPtr = new ConfigureCommand(req->name, in, out);
delta.add(cmdPtr);
- enqueue(std::pair<int, Change>(req.envId, delta));
- res.success = true;
+ enqueue(std::pair<int, Change>(req->envid, delta));
+ res->success = true;
return;
}
-void ConfigurationManager::addTargetCallback(const std::shared_ptr<dyknow_manager::AddTarget::Request> req, std::shared_ptr<dyknow_manager::AddTarget::Response> res) {
+void ConfigurationManager::addTargetCallback(const std::shared_ptr<dyknow_interfaces::srv::AddTarget::Request> req, std::shared_ptr<dyknow_interfaces::srv::AddTarget::Response> res) {
Change delta;
- for (auto& target : req.targets) {
+ 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;
+ enqueue(std::pair<int, Change>(req->envid, delta));
+ res->success = true;
return;
}
-void ConfigurationManager::removeTargetCallback(const std::shared_ptr<dyknow_manager::RemoveTarget::Request> req, std::shared_ptr<dyknow_manager::RemoveTarget::Response> res) {
+void ConfigurationManager::removeTargetCallback(const std::shared_ptr<dyknow_interfaces::srv::RemoveTarget::Request> req, std::shared_ptr<dyknow_interfaces::srv::RemoveTarget::Response> res) {
Change delta;
- for (auto& label : req.targets) {
+ for (auto& label : req->targets) {
RemoveTargetCommand* cmdPtr = new RemoveTargetCommand(label);
delta.add(cmdPtr);
}
- enqueue(std::pair<int, Change>(req.envId, delta));
- res.success = true;
+ enqueue(std::pair<int, Change>(req->envid, delta));
+ res->success = true;
return;
}
-void ConfigurationManager::refreshCallback(const std::shared_ptr<dyknow_manager::Refresh::Request> req, std::shared_ptr<dyknow_manager::Refresh::Response> res) {
- ROS_INFO("Scheduling refresh");
+void ConfigurationManager::refreshCallback(const std::shared_ptr<dyknow_interfaces::srv::Refresh::Request> req, std::shared_ptr<dyknow_interfaces::srv::Refresh::Response> res) {
+ RCLCPP_INFO(this->nh->get_logger(), "Scheduling refresh");
Change delta;
RefreshCommand* cmdPtr = new RefreshCommand();
delta.add(cmdPtr);
- enqueue(std::pair<int, Change>(req.envId, delta));
- res.success = true;
+ enqueue(std::pair<int, Change>(req->envid, delta));
+ res->success = true;
return;
}
-void ConfigurationManager::repairCallback(const std::shared_ptr<dyknow_manager::Repair::Request> req, std::shared_ptr<dyknow_manager::Repair::Response> res) {
+void ConfigurationManager::repairCallback(const std::shared_ptr<dyknow_interfaces::srv::Repair::Request> req, std::shared_ptr<dyknow_interfaces::srv::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;
+ // 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;
+ // }
+
+ std::vector<Environment>::iterator iter;
+ for(iter = environments.begin(); iter != environments.end(); iter++) {
+ if(req->envid == iter->getId()) {
+ iter->repair = true;
+ bool result = true;
+ res->success = result;
+ return;
+ }
}
- ROS_WARN("Unable to find environment: %i", req.envId);
- res.success = false;
+ RCLCPP_WARN(this->nh->get_logger(), "Unable to find environment: %i", req->envid);
+ res->success = false;
return;
}
-void ConfigurationManager::getValidInputsCallback(const std::shared_ptr<dyknow_manager::GetValidInputs::Request> req, std::shared_ptr<dyknow_manager::GetValidInputs::Response> res) {
+void ConfigurationManager::getValidInputsCallback(const std::shared_ptr<dyknow_interfaces::srv::GetValidInputs::Request> req, std::shared_ptr<dyknow_interfaces::srv::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;
- }
- }
+ std::vector<Environment>::iterator iter;
+ for(iter = environments.begin(); iter != environments.end(); iter++) {
+ if(req->env == iter->getId()) {
- if(id < 0) {
- ROS_WARN("Unable to find transformation with label %s; skipping", req.transformation.c_str());
- res.success = false;
- return;
- }
-
- 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);
+ int id = -1;
+ for(int i = 0; i < iter->getMatrix().library.size(); i++) {
+ if(iter->getMatrix().library[i].label == req->transformation) {
+ id = iter->getMatrix().library[i].id;
+ break;
+ }
}
- for (auto& tag : spec.tags) {
- dyknow_manager::Tag elem;
- elem.port = tag.first;
- elem.tag = tag.second;
- tf.tags.push_back(elem);
+ if(id < 0) {
+ RCLCPP_WARN(this->nh->get_logger(),"Unable to find transformation with label %s; skipping", req->transformation.c_str());
+ res->success = false;
+ return;
}
- res.transformations.push_back(tf);
- res.ports.push_back(port.second);
+ //Port port;
+ for_each(iter->getValidInputs(Port(id, req->port-1)).begin(), iter->getValidInputs(Port(id, req->port-1)).end(), [this, iter, req, res](Port port) {
+ RCLCPP_INFO(this->nh->get_logger(),"getValidInputs");
+ TransformationSpec spec = iter->getMatrix().decode(port);
+
+ dyknow_interfaces::msg::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";
+
+ //TransformationParam param;
+ for_each(spec.parameters.begin(), spec.parameters.end(), [&tf](TransformationParam param) {
+ dyknow_interfaces::msg::Parameter elem;
+ elem.name = param.name;
+ elem.type = param.type;
+ elem.value = param.value;
+ tf.params.push_back(elem);
+ });
+
+ //TransformationTag tag;
+ for_each(spec.tags.begin(), spec.tags.end(), [&tf](TransformationTag tag) {
+ dyknow_interfaces::msg::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;
}
-
- res.success = true;
- return;
}
- ROS_WARN("Unable to find environment: %i", req.env);
+ RCLCPP_WARN(this->nh->get_logger(), "Unable to find environment: %i", req->env);
return;
}
-void ConfigurationManager::expPopulateCallback(const std::shared_ptr<dyknow_manager::ExpPopulate::Request> req, std::shared_ptr<dyknow_manager::ExpPopulate::Response> res) {
+void ConfigurationManager::expPopulateCallback(const std::shared_ptr<dyknow_interfaces::srv::ExpPopulate::Request> req, std::shared_ptr<dyknow_interfaces::srv::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;
+ std::vector<Environment>::iterator iter;
+ for(iter = environments.begin(); iter != environments.end(); iter++) {
+ if(req->env == iter->getId()) {
+ iter->expPopulate(req->seed, req->numtransform, req->density, req->lambdafunction, req->numtarget);
+ res->success = true;
+ return;
+ }
}
- ROS_WARN("Unable to find environment: %i", req.env);
+ RCLCPP_WARN(this->nh->get_logger(), "Unable to find environment: %i", req->env);
return;
}
void ConfigurationManager::spin() {
- while(ros::ok()) {
+ while(rclcpp::ok()) {
processRepairs();
if(empty()) {
- ros::Duration(1.0).sleep();
+ rclcpp::sleep_for(std::chrono::seconds(1));
+ //ros::Duration(1.0).sleep();
} else {
std::pair<int, Change> changePair = dequeue();
apply(changePair.second, changePair.first);
@@ -280,22 +293,21 @@ void ConfigurationManager::spin() {
}
}
-void ConfigurationManager::apply(Change delta, int environment) {
+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);
+ std::vector<Environment>::iterator iter;
+ for(iter = environments.begin(); iter != environments.end(); iter++) {
+ if(environment == iter->getId()) {
+ return delta.execute(&*iter);
+ }
}
- ROS_WARN("Unable to find environment: %i", environment);
- return;
+ RCLCPP_WARN(this->nh->get_logger(), "Unable to find environment: %i", environment);
+ return false;
}
-void ConfigurationManager::empty() {
+bool ConfigurationManager::empty() {
std::lock_guard<std::mutex> guard(deltaAccess);
return changeQueue.empty();
}
diff --git a/src/dyknow_manager/src/environment.cpp b/src/dyknow_manager/src/environment.cpp
index 6207730..826b7ec 100644
--- a/src/dyknow_manager/src/environment.cpp
+++ b/src/dyknow_manager/src/environment.cpp
@@ -1,9 +1,9 @@
-#include "environment.hpp"
+#include "dyknow_manager/environment.hpp"
namespace dyknow {
bool Environment::addTransformation(TransformationSpec spec) {
- ROS_INFO("Adding transformation %s (cost: %f)", spec.label.c_str(), spec.cost);
+ RCLCPP_INFO(this->nh->get_logger(),"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;
@@ -14,7 +14,7 @@ bool Environment::addTransformation(TransformationSpec spec) {
}
bool Environment::removeTransformation(std::string label) {
- ROS_INFO("Removing transformation %s", label.c_str());
+ RCLCPP_INFO(this->nh->get_logger(),"Removing transformation %s", label.c_str());
std::string s;
// Remove transformation from library and invalidate in matrix
@@ -41,14 +41,14 @@ bool Environment::spawn(std::string name, std::string type, bool protect) {
args.push_back(param.value); //FIXME: The interface currently ignores parameter names and types
}
- if (CompUnitHelper::spawn(h, nodeletManagerName, name, iter->source, args)) {
+ 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());
+ RCLCPP_INFO(this->nh->get_logger(), "** %s is now protected **", name.c_str());
}
return true;
}
@@ -69,7 +69,7 @@ bool Environment::destroy(std::string name) {
return true;
}
} else {
- ROS_WARN("Unrecognised CU registered for unloading: %s", name.c_str());
+ RCLCPP_WARN(this->nh->get_logger(), "Unrecognised CU registered for unloading: %s", name.c_str());
}
return false;
@@ -87,7 +87,7 @@ bool Environment::configure(std::string name, std::map<std::string, std::string>
return true;
}
} else {
- ROS_WARN("Unrecognised CU registered for reconfiguration: %s", name.c_str());
+ RCLCPP_WARN(this->nh->get_logger(), "Unrecognised CU registered for reconfiguration: %s", name.c_str());
}
return false;
@@ -103,22 +103,22 @@ bool Environment::addTarget(TargetSpec spec) {
spec.id = iter->id;
targets.push_back(spec);
this->repair = true;
- ROS_INFO("Adding target %i (%s)", spec.id, spec.label.c_str());
+ RCLCPP_INFO(this->nh->get_logger(), "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());
+ RCLCPP_WARN(this->nh->get_logger(), "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());
+ RCLCPP_INFO(this->nh->get_logger(), "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());
+ RCLCPP_INFO(this->nh->get_logger(), "Removing target %s", label.c_str());
auto iter = std::remove_if(targets.begin(), targets.end(), [&label](const TargetSpec& ts) {
return ts.label == label;
@@ -127,14 +127,14 @@ bool Environment::removeTarget(std::string label) {
if (iter != targets.end()) {
targets.erase(iter, targets.end());
} else {
- ROS_WARN("Unrecognised target registered for removal: %s", label.c_str());
+ RCLCPP_WARN(this->nh->get_logger(), "Unrecognised target registered for removal: %s", label.c_str());
}
return true;
}
bool Environment::refresh() {
- ROS_INFO("Refreshing feasibility matrix");
+ RCLCPP_INFO(this->nh->get_logger(), "Refreshing feasibility matrix");
this->matrix = FeasibilityMatrix(this->transformations, 0.01);
return true;
}
diff --git a/src/dyknow_manager/src/manager_componet.cpp b/src/dyknow_manager/src/manager_componet.cpp
index 503b6bd..297db62 100644
--- a/src/dyknow_manager/src/manager_componet.cpp
+++ b/src/dyknow_manager/src/manager_componet.cpp
@@ -7,9 +7,13 @@ namespace manager {
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::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/transformation_spec.cpp b/src/dyknow_manager/src/transformation_spec.cpp
index d548bfd..0d17d46 100644
--- a/src/dyknow_manager/src/transformation_spec.cpp
+++ b/src/dyknow_manager/src/transformation_spec.cpp
@@ -1,4 +1,4 @@
-#include "transformation_spec.hpp"
+#include "dyknow_manager/transformation_spec.hpp"
namespace dyknow {
@@ -7,7 +7,7 @@ TransformationSpec TransformationFactory::create(std::string path) {
return TransformationSpec();
}
-TransformationSpec TransformationFactory::create(dyknow_manager::Transformation tfMsg) {
+TransformationSpec TransformationFactory::create(dyknow_interfaces::msg::Transformation tfMsg) {
TransformationSpec spec;
spec.label = tfMsg.label;
if (tfMsg.type.compare("nodelet") == 0) {
diff --git a/src/dyknow_nodehandle/include/dyknow_nodehandle/node.hpp b/src/dyknow_nodehandle/include/dyknow_nodehandle/node.hpp
index 0ea833c..be401cd 100644
--- a/src/dyknow_nodehandle/include/dyknow_nodehandle/node.hpp
+++ b/src/dyknow_nodehandle/include/dyknow_nodehandle/node.hpp
@@ -24,7 +24,6 @@ public:
Node(const std::string & nodeName, const rclcpp::NodeOptions & options) : rclcpp::Node(nodeName, options) , subCount(0), pubCount(0), reconfigCount(0), reconfigCallbackFn(NULL) {
-
configureService = this->create_service<dyknow_interfaces::srv::Configure>(
"set_config", std::bind(&Node::configureCallback, this, std::placeholders::_1, std::placeholders::_2));
getConfigService = this->create_service<dyknow_interfaces::srv::GetConfig>(
@@ -38,8 +37,6 @@ public:
std::function<void(std::map<std::string, std::string>, std::map<std::string, std::string>)> reconfigCallbackFn;
-
-
template<class M, class T>
dyknow::Subscriber create_subscription(uint32_t queueSize, void(T::*fp)(std::shared_ptr<M>&), T* obj) {
std::stringstream ss;
--
GitLab