Environment manager base architecture implemented

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;
+}