Environment manager base architecture implemented

env_manager/include/component_manager/client_component.hpp

@@ -0,0 +1,61 @@
+#ifndef ENV_MANAGER__COMPONENT_MANAGER__CLIENT_COMPONENT_HPP_
+#define ENV_MANAGER__COMPONENT_MANAGER__CLIENT_COMPONENT_HPP_
+
+#include "component_manager/visibility_control.h"
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <chrono>
+#include <cstdlib>
+#include <future>
+#include <memory>
+
+
+namespace env_manager
+{
+namespace component_manager
+{
+
+using namespace std::chrono_literals;
+
+const std::string DEFAULT_CLIENT_NODE_NAME = "env_manager_client_node";
+const std::string DEFAULT_CLIENT_NAME = "client";
+
+template <typename ServiceT>
+class ClientComponent: public rclcpp::Node
+{
+public:
+    explicit ClientComponent(const rclcpp::NodeOptions& options)
+        : Node(DEFAULT_CLIENT_NODE_NAME, options)
+    {
+        _client = create_client<ServiceT>(DEFAULT_CLIENT_NAME, 10);
+    }
+
+    virtual void callback(
+            typename rclcpp::Client<ServiceT>::SharedFuture future) = 0;
+
+    void populate_request(
+            const std::shared_ptr<typename ServiceT::Request>& request)
+    {
+        while (!_client->wait_for_service(1s)) 
+        {
+            if (rclcpp::ok()) 
+            {
+                RCLCPP_ERROR(this->get_logger(),
+                "Client interrupted while waiting for service. Terminating...");
+            }
+        }
+
+        auto result_future = _client->async_send_request(
+            request, std::bind(&ClientComponent::callback, 
+                               this, std::placeholders::_1));
+    }
+
+private:
+    typename rclcpp::Client<ServiceT>::SharedPtr _client;
+};
+
+} // namespace component_manager
+} // namespace env_manager
+
+#endif // ENV_MANAGER__COMPONENT_MANAGER__CLIENT_COMPONENT_HPP_

env_manager/include/component_manager/component_manager.hpp

@@ -0,0 +1,50 @@
+#ifndef COMPONENT_MANAGER__COMPONENT_MANAGER_HPP_
+#define COMPONENT_MANAGER__COMPONENT_MANAGER_HPP_
+
+#include "component_manager/visibility_control.h"
+#include "config/options.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_components/component_manager.hpp>
+#include <rclcpp_components/node_factory.hpp>
+#include <class_loader/class_loader.hpp>
+
+#include <glog/logging.h>
+
+
+namespace env_manager
+{
+namespace component_manager
+{
+
+/**
+ * This class implements the system for managing and configuring loaded components. 
+ * 
+ * It is assumed that the loaded components are inherited from the classes 
+ *  provided in the library for each node type. 
+ */
+class ComponentManager
+{
+public:
+    ComponentManager(std::weak_ptr<rclcpp::Executor> executor);
+
+    void register_components(
+            const std::map<std::string, config::ComponentOption> &comps, 
+            const std::map<std::string, config::NodeOption> &nodes,
+            const std::string& ns);
+
+    void remove_components_from_executor();
+
+    void remap_components_namespace(const std::string& ns);
+
+private:
+    std::weak_ptr<rclcpp::Executor> _executor;
+    std::vector<class_loader::ClassLoader* > _loaders;
+    std::map<std::string, rclcpp_components::NodeInstanceWrapper> _node_wrappers;
+    std::map<std::string, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr> _nodes;
+};
+
+} // namespace env_manager
+} // namespace component_manager
+
+#endif  // COMPONENT_MANAGER__COMPONENT_MANAGER_HPP_

env_manager/include/component_manager/publisher_component.hpp

@@ -0,0 +1,46 @@
+#ifndef ENV_MANAGER__COMPONENT_MANAGER__PUBLISHER_COMPONENT_HPP_
+#define ENV_MANAGER__COMPONENT_MANAGER__PUBLISHER_COMPONENT_HPP_
+
+#include "component_manager/visibility_control.h"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace env_manager
+{
+namespace component_manager
+{
+
+const std::string DEFAULT_PUB_NODE_NAME = "env_manager_pub_node";
+const std::string DEFAULT_PUB_TOPIC_NAME = "pub_topic";
+
+template <typename MessageT>
+class PublisherComponent: public rclcpp::Node
+{
+public:
+    explicit PublisherComponent(const rclcpp::NodeOptions& options)
+        : Node(DEFAULT_PUB_NODE_NAME, options)
+    {
+        _pub = create_publisher<MessageT>(DEFAULT_PUB_TOPIC_NAME, 10);
+        auto ret = rcutils_logging_set_logger_level(
+            get_logger().get_name(), RCUTILS_LOG_SEVERITY_FATAL);
+        if (ret != RCUTILS_RET_OK) 
+        {
+            RCLCPP_ERROR(get_logger(), 
+                "Error setting severity: %s", rcutils_get_error_string().str);
+            rcutils_reset_error();
+        }
+    }
+
+    void populate_publication(const MessageT& msg)
+    {
+        _pub->publish(std::move(msg));
+    }
+
+private:
+    typename rclcpp::Publisher<MessageT>::SharedPtr _pub;
+};
+
+} // namespace component_manager
+} // namespace env_manager
+
+#endif // ENV_MANAGER__COMPONENT_MANAGER__PUBLISHER_COMPONENT_HPP_

env_manager/include/component_manager/service_component.hpp

@@ -0,0 +1,39 @@
+#ifndef ENV_MANAGER__COMPONENT_MANAGER__SERVICE_COMPONENT_HPP_
+#define ENV_MANAGER__COMPONENT_MANAGER__SERVICE_COMPONENT_HPP_
+
+#include "component_manager/visibility_control.h"
+
+#include <rclcpp/node.hpp>
+
+namespace env_manager
+{
+namespace component_manager
+{
+
+const std::string DEFAULT_SERVICE_NDOE_NAME = "env_manager_service_node";
+const std::string DEFAULT_SERVICE_NAME = "service";
+
+template <typename ServiceT>
+class ServiceComponent: public rclcpp::Node
+{
+public:
+    explicit ServiceComponent(const rclcpp::NodeOptions& options)
+        : Node(DEFAULT_SERVICE_NDOE_NAME, options)
+    {
+        _service = create_service<ServiceT>(
+            DEFAULT_SERVICE_NAME, std::bind(
+                &ServiceComponent::callback, this, 
+                std::placeholders::_1, std::placeholders::_2));
+    }
+
+    virtual void callback(std::shared_ptr<typename ServiceT::Request> request,
+        std::shared_ptr<typename ServiceT::Response> response) = 0;
+
+private:
+    typename rclcpp::Service<ServiceT>::SharedPtr _service;
+};
+
+} // namespace component_manager
+} // namespace env_manager
+
+#endif // ENV_MANAGER__COMPONENT_MANAGER__SERVICE_COMPONENT_HPP_

env_manager/include/component_manager/subscriber_component.hpp

@@ -0,0 +1,44 @@
+#ifndef ENV_MANAGER_COMPONENT_MANAGER_SUBSCRIBER_COMPONENT_HPP_
+#define ENV_MANAGER_COMPONENT_MANAGER_SUBSCRIBER_COMPONENT_HPP_
+
+#include <iostream>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace env_manager
+{
+namespace component_manager
+{
+
+const std::string DEFAULT_SUB_NODE_NAME = "env_manager_sub_node";
+const std::string DEFAULT_SUB_TOPIC_NAME = "sub_topic";
+
+template <typename MessageT>
+class SubscriberComponent : public rclcpp::Node
+{
+public:
+    explicit SubscriberComponent(const rclcpp::NodeOptions& options)
+        : Node(DEFAULT_SUB_NODE_NAME, options)
+    {
+        _sub = create_subscription<MessageT>(
+                    DEFAULT_SUB_TOPIC_NAME, 10, this->callback);
+        auto ret = rcutils_logging_set_logger_level(
+            get_logger().get_name(), RCUTILS_LOG_SEVERITY_FATAL);
+        if (ret != RCUTILS_RET_OK) 
+        {
+            RCLCPP_ERROR(get_logger(), "Error setting severity: %s", rcutils_get_error_string().str);
+            rcutils_reset_error();
+        }
+    }
+
+    virtual void callback(const MessageT& msg) = 0;
+
+private:
+    typename rclcpp::Subscription<MessageT>::SharedPtr _sub;
+};
+
+} // namespace component_manager
+} // namespace env_manager
+
+#endif // ENV_MANAGER_COMPONENT_MANAGER_SUBSCRIBER_COMPONENT_HPP_

env_manager/include/component_manager/visibility_control.h

@@ -0,0 +1,35 @@
+#ifndef COMPONENT_MANAGER__VISIBILITY_CONTROL_H_
+#define COMPONENT_MANAGER__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define COMPONENT_MANAGER_EXPORT __attribute__ ((dllexport))
+    #define COMPONENT_MANAGER_IMPORT __attribute__ ((dllimport))
+  #else
+    #define COMPONENT_MANAGER_EXPORT __declspec(dllexport)
+    #define COMPONENT_MANAGER_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef COMPONENT_MANAGER_BUILDING_LIBRARY
+    #define COMPONENT_MANAGER_PUBLIC COMPONENT_MANAGER_EXPORT
+  #else
+    #define COMPONENT_MANAGER_PUBLIC COMPONENT_MANAGER_IMPORT
+  #endif
+  #define COMPONENT_MANAGER_PUBLIC_TYPE COMPONENT_MANAGER_PUBLIC
+  #define COMPONENT_MANAGER_LOCAL
+#else
+  #define COMPONENT_MANAGER_EXPORT __attribute__ ((visibility("default")))
+  #define COMPONENT_MANAGER_IMPORT
+  #if __GNUC__ >= 4
+    #define COMPONENT_MANAGER_PUBLIC __attribute__ ((visibility("default")))
+    #define COMPONENT_MANAGER_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define COMPONENT_MANAGER_PUBLIC
+    #define COMPONENT_MANAGER_LOCAL
+  #endif
+  #define COMPONENT_MANAGER_PUBLIC_TYPE
+#endif
+
+#endif  // COMPONENT_MANAGER__VISIBILITY_CONTROL_H_