Environment manager base architecture implemented

.gitlab-ci.yml

@@ -13,6 +13,9 @@ build-colcon-job:
   stage: build
   script:
   - apt-get update
+  - apt-get install -y make wget libgoogle-glog-dev libreadline-dev
+  - wget http://www.lua.org/ftp/lua-5.3.1.tar.gz && tar zxf lua-5.3.1.tar.gz
+  - make -C lua-5.3.1 linux && make -C lua-5.3.1 install
   - mkdir -p .src/robossembler-ros2
   - mv * .src/robossembler-ros2
   - mv .git .src/robossembler-ros2

env_manager/CMakeLists.txt

@@ -0,0 +1,126 @@
+cmake_minimum_required(VERSION 3.5)
+project(env_manager)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+set(CMAKE_CXX_STANDARD_REQUIRED True)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(ENV_MANAGER_CONFIGURATION_FILES_DIRECTORY ${PROJECT_SOURCE_DIR}/config
+    CACHE PATH ".lua configuration files dir")
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(Boost REQUIRED)
+find_package(rcl REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(rcutils REQUIRED)
+find_package(glog 0.4.0 REQUIRED)
+find_package(Lua 5.3 REQUIRED)
+
+
+add_library(env_manager STATIC 
+    src/component_manager/component_manager.cpp
+    src/config/config_file_resolver.cpp
+    src/config/lua_file_resolver.cpp
+    src/config/options.cpp
+    )
+
+configure_file(
+  ${PROJECT_SOURCE_DIR}/include/config/config.hpp.cmake
+  ${PROJECT_BINARY_DIR}/include/config/config.hpp)
+
+target_include_directories(env_manager PUBLIC 
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(
+  env_manager
+  Boost
+  rcl
+  glog
+  Lua
+  class_loader
+  rclcpp
+  rclcpp_components
+)
+
+target_include_directories(env_manager PUBLIC 
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(env_manager PRIVATE "COMPONENT_MANAGER_BUILDING_LIBRARY")
+
+if(NOT WIN32)
+  ament_environment_hooks(
+    "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}")
+endif()
+
+add_executable(main
+    src/main.cpp
+    )
+target_link_libraries(main env_manager lua glog)
+ament_target_dependencies(
+    main
+    Boost
+    rcl
+    rclcpp
+    glog
+    Lua
+    )
+
+install(DIRECTORY config DESTINATION config)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+install(
+  TARGETS env_manager 
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+    TARGETS main
+    DESTINATION lib/${PROJECT_NAME}
+    )
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  env_manager
+)
+ament_export_targets(
+  export_${PROJECT_NAME}
+)
+ament_export_dependencies(rcutils)
+
+ament_package()

env_manager/config/config.lua

@@ -0,0 +1,19 @@
+-- env_manager configuraiton file
+include "nodes.lua"
+include "utils/utils.lua"
+
+-- include environments configs
+include "environments/simulator.lua"
+include "environments/ground_true.lua"
+
+-- env_manager configuration
+env_manager = {
+    environments = { 
+        SIMULATOR, GROUND_TRUE 
+    },
+    nodes = NODES 
+}
+
+check_nodes(env_manager)
+
+return env_manager

env_manager/config/environments/ground_true.lua

@@ -0,0 +1,14 @@
+-- environment configuraiton
+GROUND_TRUE = {
+    namespace = "ground_true",
+    components = {
+        talker_node = {
+            lib = "libpub_component.so",
+            class = "pub_component::Publisher",
+        },
+        service_node = {
+            lib = "libsrv_component.so",
+            class = "srv_component::Service"
+        }
+    }
+}

env_manager/config/environments/simulator.lua

@@ -0,0 +1,14 @@
+-- environment configuration
+SIMULATOR = {
+    namespace = "simulator_env",
+    components = {
+        talker_node = {
+            lib = "libpub_component.so",
+            class = "pub_component::Publisher",
+        },
+        service_node = {
+            lib = "libsrv_component.so",
+            class = "srv_component::Service"
+        }
+    }
+}

env_manager/config/nodes.lua

@@ -0,0 +1,12 @@
+NODES = {
+    talker_node = {
+        name = "talker",
+        type = "Publisher",
+        msg_type = "std_msgs/String",
+    },
+    service_node = {
+        name = "add_two_ints",
+        type = "Service",
+        msg_type = "example_interfaces/AddTwoInts"
+    }
+}

env_manager/config/utils/utils.lua

@@ -0,0 +1,9 @@
+function check_nodes(config)
+    for env, cfg in pairs(config.environments) do 
+        for comp, opt in pairs(cfg.components) do 
+            assert(config.nodes[comp] ~= nil, "not all nodes presented.")
+            assert(opt.lib ~= nil, "not library provided.")
+            assert(opt.class ~= nil, "not class provided.")
+        end
+    end
+end

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_

env_manager/include/config/config.hpp.cmake

@@ -0,0 +1,15 @@
+#ifndef ENV_MANAGER_CONFIG_CONFIG_H_
+#define ENV_MANAGER_CONFIG_CONFIG_H_
+
+namespace env_manager 
+{
+namespace config 
+{
+
+constexpr char kConfigurationFilesDirectory[] = "@ENV_MANAGER_CONFIGURATION_FILES_DIRECTORY@";
+constexpr char kSourceDirectory[] = "@PROJECT_SOURCE_DIR@";
+
+}  // namespace config
+}  // namespace env_manager
+
+#endif  // ENV_MANAGER_CONFIG_CONFIG_H_

env_manager/include/config/config_file_resolver.hpp

@@ -0,0 +1,28 @@
+#ifndef ENV_MANAGER_CONFIG_CONFIG_FILE_RESOLVER_H_
+#define ENV_MANAGER_CONFIG_CONFIG_FILE_RESOLVER_H_
+
+#include "lua_file_resolver.hpp"
+
+namespace env_manager
+{
+namespace config
+{
+
+class ConfigurationFileResolver : public FileResolver
+{
+public:
+    explicit ConfigurationFileResolver(
+        const std::vector<std::string>& cfg_files_dirs);
+
+    std::string GetPath(const std::string& basename) const;
+    std::string GetContent(const std::string& basename) const;
+
+private:
+    std::vector<std::string> _config_files_dirs;
+
+};
+
+} // namespace config
+} // namespace env_manager
+
+#endif // ENV_MANAGER_CONFIG_CONFIG_FILE_RESOLVER_H_

env_manager/include/config/lua_file_resolver.hpp

@@ -0,0 +1,135 @@
+#ifndef ENV_MANAGER_CONFIG_LUA_FILE_RESOLVER_H_
+#define ENV_MANAGER_CONFIG_LUA_FILE_RESOLVER_H_
+
+extern "C" {
+
+#include <lua.h>
+#include <lauxlib.h>
+#include <lualib.h>
+
+}
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace env_manager
+{
+namespace config
+{
+
+class FileResolver
+{
+public:
+    virtual ~FileResolver() {}
+    virtual std::string GetPath(const std::string& basename) const = 0;
+    virtual std::string GetContent(const std::string& basename) const = 0;
+};
+
+class LuaParameterDictionary {
+ public:
+  // Constructs the dictionary from a Lua Table specification.
+  LuaParameterDictionary(const std::string& code,
+                         std::unique_ptr<FileResolver> file_resolver);
+
+  LuaParameterDictionary(const LuaParameterDictionary&) = delete;
+  LuaParameterDictionary& operator=(const LuaParameterDictionary&) = delete;
+
+  // Constructs a LuaParameterDictionary without reference counting.
+  static std::unique_ptr<LuaParameterDictionary> NonReferenceCounted(
+      const std::string& code, std::unique_ptr<FileResolver> file_resolver);
+
+  ~LuaParameterDictionary();
+
+  // Returns all available keys.
+  std::vector<std::string> GetKeys() const;
+
+  // Returns true if the key is in this dictionary.
+  bool HasKey(const std::string& key) const;
+
+  // These methods CHECK() that the 'key' exists.
+  std::string GetString(const std::string& key);
+  double GetDouble(const std::string& key);
+  int GetInt(const std::string& key);
+  bool GetBool(const std::string& key);
+  std::unique_ptr<LuaParameterDictionary> GetDictionary(const std::string& key);
+
+  // Gets an int from the dictionary and CHECK()s that it is non-negative.
+  int GetNonNegativeInt(const std::string& key);
+
+  // Returns a string representation for this LuaParameterDictionary.
+  std::string ToString() const;
+
+  // Returns the values of the keys '1', '2', '3' as the given types.
+  std::vector<double> GetArrayValuesAsDoubles();
+  std::vector<std::string> GetArrayValuesAsStrings();
+  std::vector<std::unique_ptr<LuaParameterDictionary>>
+  GetArrayValuesAsDictionaries();
+
+ private:
+  enum class ReferenceCount { YES, NO };
+  LuaParameterDictionary(const std::string& code,
+                         ReferenceCount reference_count,
+                         std::unique_ptr<FileResolver> file_resolver);
+
+  // For GetDictionary().
+  LuaParameterDictionary(lua_State* L, ReferenceCount reference_count,
+                         std::shared_ptr<FileResolver> file_resolver);
+
+  // Function that recurses to keep track of indent for ToString().
+  std::string DoToString(const std::string& indent) const;
+
+  // Pop the top of the stack and CHECKs that the type is correct.
+  double PopDouble() const;
+  int PopInt() const;
+  bool PopBool() const;
+
+  // Pop the top of the stack and CHECKs that it is a string. The returned value
+  // is either quoted to be suitable to be read back by a Lua interpretor or
+  // not.
+  enum class Quoted { YES, NO };
+  std::string PopString(Quoted quoted) const;
+
+  // Creates a LuaParameterDictionary from the Lua table at the top of the
+  // stack, either with or without reference counting.
+  std::unique_ptr<LuaParameterDictionary> PopDictionary(
+      ReferenceCount reference_count) const;
+
+  // CHECK() that 'key' is in the dictionary.
+  void CheckHasKey(const std::string& key) const;
+
+  // CHECK() that 'key' is in this dictionary and reference it as being used.
+  void CheckHasKeyAndReference(const std::string& key);
+
+  // If desired, this can be called in the destructor of a derived class. It
+  // will CHECK() that all keys defined in the configuration have been used
+  // exactly once and resets the reference counter.
+  void CheckAllKeysWereUsedExactlyOnceAndReset();
+
+  // Reads a file into a Lua string.
+  static int LuaRead(lua_State* L);
+
+  // Handles inclusion of other Lua files and prevents double inclusion.
+  static int LuaInclude(lua_State* L);
+
+  lua_State* L_;  // The name is by convention in the Lua World.
+  int index_into_reference_table_;
+
+  // This is shared with all the sub dictionaries.
+  const std::shared_ptr<FileResolver> file_resolver_;
+
+  // If true will check that all keys were used on destruction.
+  const ReferenceCount reference_count_;
+
+  // This is modified with every call to Get* in order to verify that all
+  // parameters are read exactly once.
+  std::map<std::string, int> reference_counts_;
+
+  // List of all included files in order of inclusion. Used to prevent double
+  // inclusion.
+  std::vector<std::string> included_files_;
+};
+} // namespce config
+} // namespace env_manager
+
+#endif // ENV_MANAGER_CONFIG_LUA_FILE_RESOLVER_H_

env_manager/include/config/options.hpp

@@ -0,0 +1,57 @@
+#ifndef  ENV_MANAGER_CONFIG_OPTIONS_HPP_
+#define ENV_MANAGER_CONFIG_OPTIONS_HPP_
+
+#include "config/lua_file_resolver.hpp"
+
+#include <unordered_map>
+
+namespace env_manager
+{
+namespace config
+{
+
+struct ComponentOption
+{
+    static ComponentOption create_option(
+        LuaParameterDictionary* lua_parameter_dictionary);
+    std::string library;
+    std::string class_name;
+};
+
+struct EnvironmentOption
+{
+    static EnvironmentOption create_option(
+        LuaParameterDictionary* lua_parameter_dictionary);
+    std::string ns;
+    std::map<std::string, ComponentOption> components;
+};
+
+struct NodeOption 
+{
+    enum NodeType
+    {
+        Publisher,
+        Subscriber,
+        Service,
+        Client
+    };
+
+    static NodeOption create_option(
+        LuaParameterDictionary* lua_parameter_dictionary);
+    std::string name;
+    std::string msg_type;
+    NodeType type;
+    static const std::unordered_map<std::string, NodeType> node_type_remap;
+};
+
+struct EnvManagerOption 
+{
+    static EnvManagerOption create_option(
+        LuaParameterDictionary* lua_parameter_dictionary); 
+    std::map<std::string, EnvironmentOption> environments;
+    std::map<std::string, NodeOption> nodes;
+};
+
+}
+}
+#endif // ENV_MANAGER_CONFIG_OPTIONS_HPP_

env_manager/include/env_manager/environment_node_manager.hpp

@@ -0,0 +1,30 @@
+#ifndef ENV_MANAGER__ENVIRONMENT_NODE_MANAGER_HPP_
+#define ENV_MANAGER__ENVIRONMENT_NODE_MANAGER_HPP_
+
+#include "config/options.hpp"
+
+#include <vector>
+
+#include <rclcpp/executors.hpp>
+#include <boost/thread.hpp>
+
+extern "C"
+{
+#include <rcl/context.h>
+#include <rcl/node.h>
+}
+
+namespace env_manager
+{
+
+class EnvironmentNodeManager
+{
+public:
+    EnvironmentNodeManager();
+    
+    ~EnvironmentNodeManager();
+};
+
+} // namespace env_manager
+
+#endif // ENV_MANAGER__ENVIRONMENT_NODE_MANAGER_HPP_

env_manager/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>env_manager</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="rom.andrianov1984@gmail.com">splinter1984</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <depend>boost</depend>
+  <depend>rcl</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

env_manager/src/component_manager/component_manager.cpp

@@ -0,0 +1,152 @@
+#include "component_manager/component_manager.hpp"
+#include "component_manager/publisher_component.hpp"
+#include "component_manager/subscriber_component.hpp"
+#include "component_manager/service_component.hpp"
+#include "component_manager/client_component.hpp"
+#include "glog/logging.h"
+
+#include "rcl/remap.h"
+#include <iostream>
+
+namespace env_manager
+{
+namespace component_manager
+{
+
+ComponentManager::ComponentManager(std::weak_ptr<rclcpp::Executor> executor)
+    : _executor(executor)
+{
+}
+
+static rclcpp::NodeOptions create_options(
+    const config::NodeOption &node_opts,
+    const std::string &node_name,
+    const std::string &ns)
+{
+    rclcpp::NodeOptions opts;
+    std::vector<std::string> args = {"--ros-args", 
+        "--disable-rosout-logs", 
+        "--disable-stdout-logs", 
+        "--enable-external-lib-logs",
+        "--log-level", "WARN",
+    };
+    args.push_back("-r");
+    args.push_back("__ns:=/" + ns);
+    args.push_back("-r");
+    args.push_back("__node:=" + node_name);
+    args.push_back("-r");
+    
+    switch (node_opts.type) 
+    {
+        case config::NodeOption::NodeType::Publisher:
+            args.push_back(DEFAULT_PUB_TOPIC_NAME + ":=" + node_opts.name);
+            break; 
+        case config::NodeOption::NodeType::Subscriber:
+            args.push_back(DEFAULT_SUB_TOPIC_NAME + ":=" + node_opts.name);
+            break; 
+        case config::NodeOption::NodeType::Service:
+            args.push_back(DEFAULT_SERVICE_NAME + ":=" + node_opts.name);
+            break; 
+        case config::NodeOption::NodeType::Client:
+            args.push_back(DEFAULT_CLIENT_NAME + ":=" + node_opts.name);
+            break;
+    }
+
+    opts.arguments(args);
+
+    return opts;
+}
+
+static rclcpp::NodeOptions create_default_options(const std::string &ns)
+{
+    rclcpp::NodeOptions opts;
+    opts.arguments({
+        "--ros-args", "-r", std::string("__ns:=/" + ns), 
+        "--disable-rosout-logs", 
+        "--disable-stdout-logs", 
+        "--enable-external-lib-logs",
+        "--log-level", "FATAL",
+        });
+
+    return opts;
+}
+
+void ComponentManager::register_components(
+    const std::map<std::string, config::ComponentOption> &comps, 
+    const std::map<std::string, config::NodeOption> &nodes,
+    const std::string& ns)
+{
+    if (comps.empty())
+        return; 
+
+    for (const auto &[name, comp]: comps)
+    {
+        auto class_name = "rclcpp_components::NodeFactoryTemplate<" + comp.class_name + ">";
+                
+        LOG(INFO) << "Provide lib: " << comp.library << " namespace: " + ns;
+        auto loader = new class_loader::ClassLoader(comp.library);
+        auto classes = 
+            loader->getAvailableClasses<rclcpp_components::NodeFactory>();
+        for (auto clazz: classes)
+        {
+            rclcpp::NodeOptions opts = create_default_options(ns);
+            if (clazz == class_name)
+            {
+                auto node_opts = nodes.at(name);
+                opts = create_options(node_opts, name, ns);
+            }
+            LOG(INFO) << "Create instance of class: " << clazz;
+            auto node_factory = 
+                loader->createInstance<rclcpp_components::NodeFactory>(clazz);
+            auto wrapper = node_factory->create_node_instance(opts);
+            auto node = wrapper.get_node_base_interface();
+
+            _node_wrappers.insert({name, wrapper});
+            _nodes.insert({name, node});
+            if (auto exec = _executor.lock())
+                exec->add_node(node);
+        }
+
+        _loaders.push_back(loader);
+    }
+}
+
+void ComponentManager::remap_components_namespace(const std::string &ns)
+{
+    char* nms = const_cast<char*>(ns.c_str());
+    for (auto& [name, wrapper]: _node_wrappers)
+    {
+        auto node = (rclcpp::Node*)wrapper.get_node_instance().get(); 
+        auto opts = node->get_node_options();
+        auto ret = rcl_remap_node_namespace(
+            &opts.get_rcl_node_options()->arguments, 
+            NULL, node->get_name(), rcl_get_default_allocator(), 
+            &nms); 
+        
+        if (ret == RCL_RET_OK)
+            LOG(INFO) << "Succesfully remap node with ns: " + ns;
+    }
+    
+    //std::logic_error("Not implemented." + ns);
+}
+
+void ComponentManager::remove_components_from_executor()
+{
+    if (_nodes.empty())
+    {
+        LOG(WARNING) << "Unable to remove nodes from executor.";
+        return;
+    }
+
+    if (auto exec = _executor.lock())
+    {
+        for (const auto &[node_name, node]: _nodes)
+        {
+            LOG(INFO) << "Remove node '" << node_name << "' from executor.";
+            exec->remove_node(node);
+        }
+    }
+}
+
+} // namespace env_manager
+} // namespace component_manager

env_manager/src/config/config_file_resolver.cpp

@@ -0,0 +1,51 @@
+#include "config/config_file_resolver.hpp"
+
+#include <fstream>
+#include <iostream>
+#include <streambuf>
+
+#include "config/config.hpp"
+
+#include <glog/logging.h>
+
+namespace env_manager
+{
+namespace config
+{
+
+ConfigurationFileResolver::ConfigurationFileResolver(
+    const std::vector<std::string>& cfg_files_dirs)
+    : _config_files_dirs(cfg_files_dirs)
+{
+    _config_files_dirs.push_back(kConfigurationFilesDirectory);
+}
+
+std::string ConfigurationFileResolver::GetPath(
+    const std::string &basename) const
+{
+    for (const auto& path: _config_files_dirs)
+    {
+        const std::string filename = path + "/" + basename;
+        std::ifstream stream(filename.c_str());
+        if (stream.good())
+        {
+            LOG(INFO) << "found filename: " << filename;
+            return filename;
+        }   
+    }
+
+    LOG(FATAL) << "File '" << basename << "' was not found.";
+}
+
+std::string ConfigurationFileResolver::GetContent(
+    const std::string &basename) const
+{
+    CHECK(!basename.empty()) << "File basename cannot be empty." << basename;
+    const std::string filename = GetPath(basename);
+    std::ifstream stream(filename.c_str());
+    return std::string(std::istreambuf_iterator<char>(stream),
+                       std::istreambuf_iterator<char>());
+}
+
+} // namespace config
+} // namespace env_manager

env_manager/src/config/lua_file_resolver.cpp

@@ -0,0 +1,446 @@
+#include "config/lua_file_resolver.hpp"
+#include "glog/logging.h"
+
+#include <algorithm>
+#include <cmath>
+#include <functional>
+#include <memory>
+
+namespace env_manager
+{
+namespace config
+{
+
+namespace {
+// Replace the string at the top of the stack through a quoted version that Lua
+// can read back.
+void QuoteStringOnStack(lua_State* L) {
+  CHECK(lua_isstring(L, -1)) << "Top of stack is not a string value.";
+  int current_index = lua_gettop(L);
+
+  // S: ... string
+  lua_pushglobaltable(L);         // S: ... string globals
+  lua_getfield(L, -1, "string");  // S: ... string globals <string module>
+  lua_getfield(L, -1,
+               "format");   // S: ... string globals <string module> format
+  lua_pushstring(L, "%q");  // S: ... string globals <string module> format "%q"
+  lua_pushvalue(L, current_index);  // S: ... string globals <string module>
+                                    // format "%q" string
+
+  lua_call(L, 2, 1);  // S: ... string globals <string module> quoted
+  lua_replace(L, current_index);  // S: ... quoted globals <string module>
+
+  lua_pop(L, 2);  // S: ... quoted
+}
+
+// Sets the given 'dictionary' as the value of the "this" key in Lua's registry
+// table.
+void SetDictionaryInRegistry(lua_State* L, LuaParameterDictionary* dictionary) {
+  lua_pushstring(L, "this");
+  lua_pushlightuserdata(L, dictionary);
+  lua_settable(L, LUA_REGISTRYINDEX);
+}
+
+// Gets the 'dictionary' from the "this" key in Lua's registry table.
+LuaParameterDictionary* GetDictionaryFromRegistry(lua_State* L) {
+  lua_pushstring(L, "this");
+  lua_gettable(L, LUA_REGISTRYINDEX);
+  void* return_value = lua_isnil(L, -1) ? nullptr : lua_touserdata(L, -1);
+  lua_pop(L, 1);
+  CHECK(return_value != nullptr);
+  return reinterpret_cast<LuaParameterDictionary*>(return_value);
+}
+
+// CHECK()s if a Lua method returned an error.
+void CheckForLuaErrors(lua_State* L, int status) {
+  CHECK_EQ(status, 0) << lua_tostring(L, -1);
+}
+
+// Returns 'a' if 'condition' is true, else 'b'.
+int LuaChoose(lua_State* L) {
+  CHECK_EQ(lua_gettop(L), 3) << "choose() takes (condition, a, b).";
+  CHECK(lua_isboolean(L, 1)) << "condition is not a boolean value.";
+
+  const bool condition = lua_toboolean(L, 1);
+  if (condition) {
+    lua_pushvalue(L, 2);
+  } else {
+    lua_pushvalue(L, 3);
+  }
+  return 1;
+}
+
+// Pushes a value to the Lua stack.
+void PushValue(lua_State* L, const int key) { lua_pushinteger(L, key); }
+void PushValue(lua_State* L, const std::string& key) {
+  lua_pushstring(L, key.c_str());
+}
+
+// Reads the value with the given 'key' from the Lua dictionary and pushes it to
+// the top of the stack.
+template <typename T>
+void GetValueFromLuaTable(lua_State* L, const T& key) {
+  PushValue(L, key);
+  lua_rawget(L, -2);
+}
+
+// CHECK() that the topmost parameter on the Lua stack is a table.
+void CheckTableIsAtTopOfStack(lua_State* L) {
+  CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!";
+}
+
+// Returns true if 'key' is in the table at the top of the Lua stack.
+template <typename T>
+bool HasKeyOfType(lua_State* L, const T& key) {
+  CheckTableIsAtTopOfStack(L);
+  PushValue(L, key);
+  lua_rawget(L, -2);
+  const bool key_not_found = lua_isnil(L, -1);
+  lua_pop(L, 1);  // Pop the item again.
+  return !key_not_found;
+}
+
+// Iterates over the integer keys of the table at the top of the stack of 'L•
+// and pushes the values one by one. 'pop_value' is expected to pop a value and
+// put them into a C++ container.
+void GetArrayValues(lua_State* L, const std::function<void()>& pop_value) {
+  int idx = 1;
+  while (true) {
+    GetValueFromLuaTable(L, idx);
+    if (lua_isnil(L, -1)) {
+      lua_pop(L, 1);
+      break;
+    }
+    pop_value();
+    ++idx;
+  }
+}
+
+}  // namespace
+
+std::unique_ptr<LuaParameterDictionary>
+LuaParameterDictionary::NonReferenceCounted(
+    const std::string& code, std::unique_ptr<FileResolver> file_resolver) {
+  return std::unique_ptr<LuaParameterDictionary>(new LuaParameterDictionary(
+      code, ReferenceCount::NO, std::move(file_resolver)));
+}
+
+LuaParameterDictionary::LuaParameterDictionary(
+    const std::string& code, std::unique_ptr<FileResolver> file_resolver)
+    : LuaParameterDictionary(code, ReferenceCount::YES,
+                             std::move(file_resolver)) {}
+
+LuaParameterDictionary::LuaParameterDictionary(
+    const std::string& code, ReferenceCount reference_count,
+    std::unique_ptr<FileResolver> file_resolver)
+    : L_(luaL_newstate()),
+      index_into_reference_table_(-1),
+      file_resolver_(std::move(file_resolver)),
+      reference_count_(reference_count) {
+  CHECK_NOTNULL(L_);
+  SetDictionaryInRegistry(L_, this);
+
+  luaL_openlibs(L_);
+
+  lua_register(L_, "choose", LuaChoose);
+  lua_register(L_, "include", LuaInclude);
+  lua_register(L_, "read", LuaRead);
+
+  CheckForLuaErrors(L_, luaL_loadstring(L_, code.c_str()));
+  CheckForLuaErrors(L_, lua_pcall(L_, 0, 1, 0));
+  CheckTableIsAtTopOfStack(L_);
+}
+
+LuaParameterDictionary::LuaParameterDictionary(
+    lua_State* const L, ReferenceCount reference_count,
+    std::shared_ptr<FileResolver> file_resolver)
+    : L_(lua_newthread(L)),
+      file_resolver_(std::move(file_resolver)),
+      reference_count_(reference_count) {
+  CHECK_NOTNULL(L_);
+
+  // Make sure this is never garbage collected.
+  CHECK(lua_isthread(L, -1));
+  index_into_reference_table_ = luaL_ref(L, LUA_REGISTRYINDEX);
+
+  CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!";
+  lua_xmove(L, L_, 1);  // Moves the table and the coroutine over.
+  CheckTableIsAtTopOfStack(L_);
+}
+
+LuaParameterDictionary::~LuaParameterDictionary() {
+  if (reference_count_ == ReferenceCount::YES) {
+    //CheckAllKeysWereUsedExactlyOnceAndReset();
+  }
+  if (index_into_reference_table_ > 0) {
+    luaL_unref(L_, LUA_REGISTRYINDEX, index_into_reference_table_);
+  } else {
+    lua_close(L_);
+  }
+}
+
+std::vector<std::string> LuaParameterDictionary::GetKeys() const {
+  CheckTableIsAtTopOfStack(L_);
+  std::vector<std::string> keys;
+
+  lua_pushnil(L_);  // Push the first key
+  while (lua_next(L_, -2) != 0) {
+    lua_pop(L_, 1);  // Pop value, keep key.
+    if (!lua_isnumber(L_, -1)) {
+      keys.emplace_back(lua_tostring(L_, -1));
+    }
+  }
+  return keys;
+}
+
+bool LuaParameterDictionary::HasKey(const std::string& key) const {
+  return HasKeyOfType(L_, key);
+}
+
+std::string LuaParameterDictionary::GetString(const std::string& key) {
+  CheckHasKeyAndReference(key);
+  GetValueFromLuaTable(L_, key);
+  return PopString(Quoted::NO);
+}
+
+std::string LuaParameterDictionary::PopString(Quoted quoted) const {
+  CHECK(lua_isstring(L_, -1)) << "Top of stack is not a string value.";
+  if (quoted == Quoted::YES) {
+    QuoteStringOnStack(L_);
+  }
+
+  const std::string value = lua_tostring(L_, -1);
+  lua_pop(L_, 1);
+  return value;
+}
+
+double LuaParameterDictionary::GetDouble(const std::string& key) {
+  CheckHasKeyAndReference(key);
+  GetValueFromLuaTable(L_, key);
+  return PopDouble();
+}
+
+double LuaParameterDictionary::PopDouble() const {
+  CHECK(lua_isnumber(L_, -1)) << "Top of stack is not a number value.";
+  const double value = lua_tonumber(L_, -1);
+  lua_pop(L_, 1);
+  return value;
+}
+
+int LuaParameterDictionary::GetInt(const std::string& key) {
+  CheckHasKeyAndReference(key);
+  GetValueFromLuaTable(L_, key);
+  return PopInt();
+}
+
+int LuaParameterDictionary::PopInt() const {
+  CHECK(lua_isnumber(L_, -1)) << "Top of stack is not a number value.";
+  const int value = lua_tointeger(L_, -1);
+  lua_pop(L_, 1);
+  return value;
+}
+
+bool LuaParameterDictionary::GetBool(const std::string& key) {
+  CheckHasKeyAndReference(key);
+  GetValueFromLuaTable(L_, key);
+  return PopBool();
+}
+
+bool LuaParameterDictionary::PopBool() const {
+  CHECK(lua_isboolean(L_, -1)) << "Top of stack is not a boolean value.";
+  const bool value = lua_toboolean(L_, -1);
+  lua_pop(L_, 1);
+  return value;
+}
+
+std::unique_ptr<LuaParameterDictionary> LuaParameterDictionary::GetDictionary(
+    const std::string& key) {
+  CheckHasKeyAndReference(key);
+  GetValueFromLuaTable(L_, key);
+  return PopDictionary(reference_count_);
+}
+
+std::unique_ptr<LuaParameterDictionary> LuaParameterDictionary::PopDictionary(
+    ReferenceCount reference_count) const {
+  CheckTableIsAtTopOfStack(L_);
+  std::unique_ptr<LuaParameterDictionary> value(
+      new LuaParameterDictionary(L_, reference_count, file_resolver_));
+  // The constructor lua_xmove()s the value, no need to pop it.
+  CheckTableIsAtTopOfStack(L_);
+  return value;
+}
+
+std::string LuaParameterDictionary::DoToString(
+    const std::string& indent) const {
+  std::string result = "{";
+  bool dictionary_is_empty = true;
+
+  const auto top_of_stack_to_string = [this, indent,
+                                       &dictionary_is_empty]() -> std::string {
+    dictionary_is_empty = false;
+
+    const int value_type = lua_type(L_, -1);
+    switch (value_type) {
+      case LUA_TBOOLEAN:
+        return PopBool() ? "true" : "false";
+        break;
+      case LUA_TSTRING:
+        return PopString(Quoted::YES);
+        break;
+      case LUA_TNUMBER: {
+        const double value = PopDouble();
+        if (std::isinf(value)) {
+          return value < 0 ? "-math.huge" : "math.huge";
+        } else {
+          return std::to_string(value);
+        }
+      } break;
+      case LUA_TTABLE: {
+        std::unique_ptr<LuaParameterDictionary> subdict(
+            PopDictionary(ReferenceCount::NO));
+        return subdict->DoToString(indent + "  ");
+      } break;
+      default:
+        LOG(FATAL) << "Unhandled type " << lua_typename(L_, value_type);
+    }
+  };
+
+  // Integer (array) keys.
+  for (int i = 1; i; ++i) {
+    GetValueFromLuaTable(L_, i);
+    if (lua_isnil(L_, -1)) {
+      lua_pop(L_, 1);
+      break;
+    }
+    result.append("\n");
+    result.append(indent);
+    result.append("  ");
+    result.append(top_of_stack_to_string());
+    result.append(",");
+  }
+
+  // String keys.
+  std::vector<std::string> keys = GetKeys();
+  if (!keys.empty()) {
+    std::sort(keys.begin(), keys.end());
+    for (const std::string& key : keys) {
+      GetValueFromLuaTable(L_, key);
+      result.append("\n");
+      result.append(indent);
+      result.append("  ");
+      result.append(key);
+      result.append(" = ");
+      result.append(top_of_stack_to_string());
+      result.append(",");
+    }
+  }
+  result.append("\n");
+  result.append(indent);
+  result.append("}");
+
+  if (dictionary_is_empty) {
+    return "{}";
+  }
+  return result;
+}
+
+std::string LuaParameterDictionary::ToString() const { return DoToString(""); }
+
+std::vector<double> LuaParameterDictionary::GetArrayValuesAsDoubles() {
+  std::vector<double> values;
+  GetArrayValues(L_, [&values, this] { values.push_back(PopDouble()); });
+  return values;
+}
+
+std::vector<std::unique_ptr<LuaParameterDictionary>>
+LuaParameterDictionary::GetArrayValuesAsDictionaries() {
+  std::vector<std::unique_ptr<LuaParameterDictionary>> values;
+  GetArrayValues(L_, [&values, this] {
+    values.push_back(PopDictionary(reference_count_));
+  });
+  return values;
+}
+
+std::vector<std::string> LuaParameterDictionary::GetArrayValuesAsStrings() {
+  std::vector<std::string> values;
+  GetArrayValues(L_,
+                 [&values, this] { values.push_back(PopString(Quoted::NO)); });
+  return values;
+}
+
+void LuaParameterDictionary::CheckHasKey(const std::string& key) const {
+  CHECK(HasKey(key)) << "Key '" << key << "' not in dictionary:\n"
+                     << ToString();
+}
+
+void LuaParameterDictionary::CheckHasKeyAndReference(const std::string& key) {
+  CheckHasKey(key);
+  reference_counts_[key]++;
+}
+
+void LuaParameterDictionary::CheckAllKeysWereUsedExactlyOnceAndReset() {
+  for (const auto& key : GetKeys()) {
+    CHECK_EQ(1, reference_counts_.count(key))
+        << "Key '" << key << "' was used the wrong number of times.";
+    CHECK_EQ(1, reference_counts_.at(key))
+        << "Key '" << key << "' was used the wrong number of times.";
+  }
+  reference_counts_.clear();
+}
+
+int LuaParameterDictionary::GetNonNegativeInt(const std::string& key) {
+  const int temp = GetInt(key);  // Will increase reference count.
+  CHECK_GE(temp, 0) << temp << " is negative.";
+  return temp;
+}
+
+// Lua function to run a script in the current Lua context. Takes the filename
+// as its only argument.
+int LuaParameterDictionary::LuaInclude(lua_State* L) {
+  CHECK_EQ(lua_gettop(L), 1);
+  CHECK(lua_isstring(L, -1)) << "include takes a filename.";
+
+  LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L);
+  const std::string basename = lua_tostring(L, -1);
+  const std::string filename =
+      parameter_dictionary->file_resolver_->GetPath(basename);
+  if (std::find(parameter_dictionary->included_files_.begin(),
+                parameter_dictionary->included_files_.end(),
+                filename) != parameter_dictionary->included_files_.end()) {
+    std::string error_msg =
+        "Tried to include " + filename +
+        " twice. Already included files in order of inclusion: ";
+    for (const std::string& filename : parameter_dictionary->included_files_) {
+      error_msg.append(filename);
+      error_msg.append("\n");
+    }
+    LOG(FATAL) << error_msg;
+  }
+  parameter_dictionary->included_files_.push_back(filename);
+  lua_pop(L, 1);
+  CHECK_EQ(lua_gettop(L), 0);
+
+  const std::string content =
+      parameter_dictionary->file_resolver_->GetContent(basename);
+  CheckForLuaErrors(
+      L, luaL_loadbuffer(L, content.c_str(), content.size(), filename.c_str()));
+  CheckForLuaErrors(L, lua_pcall(L, 0, LUA_MULTRET, 0));
+
+  return lua_gettop(L);
+}
+
+// Lua function to read a file into a string.
+int LuaParameterDictionary::LuaRead(lua_State* L) {
+  CHECK_EQ(lua_gettop(L), 1);
+  CHECK(lua_isstring(L, -1)) << "read takes a filename.";
+
+  LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L);
+  const std::string file_content =
+      parameter_dictionary->file_resolver_->GetContent(
+          lua_tostring(L, -1));
+  lua_pushstring(L, file_content.c_str());
+  return 1;
+}
+
+} // namespace config
+} // namespace env_manager

env_manager/src/config/options.cpp

@@ -0,0 +1,82 @@
+#include "config/options.hpp"
+
+#include "glog/logging.h"
+
+namespace env_manager
+{
+namespace config
+{
+
+ComponentOption ComponentOption::create_option(
+    LuaParameterDictionary *lua_parameter_dictionary)
+{
+    return ComponentOption{
+        lua_parameter_dictionary->GetString("lib"),
+        lua_parameter_dictionary->GetString("class"),
+    };
+}
+
+EnvironmentOption EnvironmentOption::create_option(
+    LuaParameterDictionary *lua_parameter_dictionary)
+{
+    auto comps = 
+        lua_parameter_dictionary->GetDictionary("components");
+    std::map<std::string, ComponentOption> components;
+    for (const auto &comp: comps->GetKeys())
+        components.insert({
+            comp, ComponentOption::create_option(comps->GetDictionary(comp).get())
+        });
+    
+    return EnvironmentOption{
+        lua_parameter_dictionary->GetString("namespace"),
+        components,
+    };
+}
+
+NodeOption NodeOption::create_option(
+    LuaParameterDictionary *lua_parameter_dictionary)
+{    
+    return NodeOption{
+        lua_parameter_dictionary->GetString("name"),
+        lua_parameter_dictionary->GetString("msg_type"),
+        node_type_remap.at(lua_parameter_dictionary->GetString("type")),
+    };
+}
+
+const std::unordered_map<std::string, NodeOption::NodeType> 
+NodeOption::node_type_remap = 
+    {
+        {"Publisher", NodeOption::NodeType::Publisher},
+        {"Subscriber", NodeOption::NodeType::Subscriber},
+        {"Service", NodeOption::NodeType::Service},
+        {"Client", NodeOption::NodeType::Client},
+    };
+
+EnvManagerOption EnvManagerOption::create_option(
+    LuaParameterDictionary *lua_parameter_dictionary)
+{
+    
+    auto lua_env_dictionary = lua_parameter_dictionary->GetDictionary("environments");
+    auto envs_dict = lua_env_dictionary->GetArrayValuesAsDictionaries();
+    std::map<std::string, EnvironmentOption> environments;
+    for (const auto& env: envs_dict)
+        environments.insert({
+            env->GetString("namespace"), EnvironmentOption::create_option(env.get())
+        });
+
+    auto lua_node_dictionary = lua_parameter_dictionary->GetDictionary("nodes");
+    auto nodes_keys = lua_node_dictionary->GetKeys();
+    std::map<std::string, NodeOption> nodes;
+    for (const auto& node: nodes_keys)
+        nodes.insert({
+            node, NodeOption::create_option(lua_node_dictionary->GetDictionary(node).get())
+        });
+
+    return EnvManagerOption{
+        environments,
+        nodes,
+    };
+}
+
+} // namespace config
+} // namespace env_manager

env_manager/src/env_manager/environment_node_manager.cpp

@@ -0,0 +1,6 @@
+#include "env_manager/environment_node_manager.hpp"
+
+namespace env_manager
+{
+
+} // namespace env_manager

env_manager/src/main.cpp

@@ -0,0 +1,60 @@
+#include "component_manager/component_manager.hpp"
+#include "config/config_file_resolver.hpp"
+#include "config/lua_file_resolver.hpp"
+#include "config/options.hpp"
+
+#include "config/config.hpp"
+
+#include "glog/logging.h"
+
+int main(int argc, const char* argv[])
+{
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_logtostderr = 1;
+    
+    rclcpp::init(argc, argv);
+    auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+
+    std::vector<std::string> dirs = {};
+    auto cfg_resolver = 
+        ::std::make_unique<::env_manager::config::ConfigurationFileResolver>(
+            std::vector<std::string>{
+                std::string(env_manager::config::kSourceDirectory) +
+                "/config"
+            }
+        );
+
+    auto code = cfg_resolver->GetContent("config.lua");
+    //LOG(INFO) << code;
+    ::env_manager::config::LuaParameterDictionary lua_dict(
+            code, std::move(cfg_resolver)
+        );
+
+    auto env_manager_opts = 
+        ::env_manager::config::EnvManagerOption::create_option(
+            &lua_dict
+        );
+
+    std::vector<env_manager::component_manager::ComponentManager> comp_mngrs;
+
+    for (const auto& [env, opts]: env_manager_opts.environments)
+    {
+        LOG(INFO) << "Start to initialize environment: " << env;
+        env_manager::component_manager::ComponentManager cmg(exec);
+        cmg.register_components(opts.components, env_manager_opts.nodes, opts.ns);
+        //cmg.upload_components_to_executor(&exec);
+        comp_mngrs.push_back(std::move(cmg));
+    }
+
+    //std::string ns = "/";
+    //comp_mngrs.begin()->remap_components_namespace(ns);
+
+    exec->spin();
+
+    for (auto &cmg: comp_mngrs)
+        cmg.remove_components_from_executor();
+
+    rclcpp::shutdown();
+
+    return 0;
+}

rbs_simulation/package.xml

@@ -14,7 +14,6 @@
   <depend>tf2_ros</depend>
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
-  <depend>nlohmann_json</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>