diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 3ae66d8..e5c5e0c 100644 --- a/.gitlab-ci.yml +++ b/.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 diff --git a/env_manager/CMakeLists.txt b/env_manager/CMakeLists.txt new file mode 100644 index 0000000..e547fc1 --- /dev/null +++ b/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 + $ + $) +ament_target_dependencies( + env_manager + Boost + rcl + glog + Lua + class_loader + rclcpp + rclcpp_components +) + +target_include_directories(env_manager PUBLIC + $) + +# 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() diff --git a/env_manager/README.md b/env_manager/README.md new file mode 100644 index 0000000..e69de29 diff --git a/env_manager/config/config.lua b/env_manager/config/config.lua new file mode 100644 index 0000000..3baf6c4 --- /dev/null +++ b/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 diff --git a/env_manager/config/environments/ground_true.lua b/env_manager/config/environments/ground_true.lua new file mode 100644 index 0000000..29e4e22 --- /dev/null +++ b/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" + } + } +} diff --git a/env_manager/config/environments/simulator.lua b/env_manager/config/environments/simulator.lua new file mode 100644 index 0000000..3bc4d1a --- /dev/null +++ b/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" + } + } +} diff --git a/env_manager/config/nodes.lua b/env_manager/config/nodes.lua new file mode 100644 index 0000000..f3ad7de --- /dev/null +++ b/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" + } +} diff --git a/env_manager/config/utils/utils.lua b/env_manager/config/utils/utils.lua new file mode 100644 index 0000000..ff746d1 --- /dev/null +++ b/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 diff --git a/env_manager/include/component_manager/client_component.hpp b/env_manager/include/component_manager/client_component.hpp new file mode 100644 index 0000000..07e34b2 --- /dev/null +++ b/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 +#include +#include +#include + + +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 +class ClientComponent: public rclcpp::Node +{ +public: + explicit ClientComponent(const rclcpp::NodeOptions& options) + : Node(DEFAULT_CLIENT_NODE_NAME, options) + { + _client = create_client(DEFAULT_CLIENT_NAME, 10); + } + + virtual void callback( + typename rclcpp::Client::SharedFuture future) = 0; + + void populate_request( + const std::shared_ptr& 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::SharedPtr _client; +}; + +} // namespace component_manager +} // namespace env_manager + +#endif // ENV_MANAGER__COMPONENT_MANAGER__CLIENT_COMPONENT_HPP_ diff --git a/env_manager/include/component_manager/component_manager.hpp b/env_manager/include/component_manager/component_manager.hpp new file mode 100644 index 0000000..815044d --- /dev/null +++ b/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 +#include +#include +#include + +#include + + +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 executor); + + void register_components( + const std::map &comps, + const std::map &nodes, + const std::string& ns); + + void remove_components_from_executor(); + + void remap_components_namespace(const std::string& ns); + +private: + std::weak_ptr _executor; + std::vector _loaders; + std::map _node_wrappers; + std::map _nodes; +}; + +} // namespace env_manager +} // namespace component_manager + +#endif // COMPONENT_MANAGER__COMPONENT_MANAGER_HPP_ diff --git a/env_manager/include/component_manager/component_template.hpp b/env_manager/include/component_manager/component_template.hpp new file mode 100644 index 0000000..e69de29 diff --git a/env_manager/include/component_manager/publisher_component.hpp b/env_manager/include/component_manager/publisher_component.hpp new file mode 100644 index 0000000..5cec622 --- /dev/null +++ b/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 +class PublisherComponent: public rclcpp::Node +{ +public: + explicit PublisherComponent(const rclcpp::NodeOptions& options) + : Node(DEFAULT_PUB_NODE_NAME, options) + { + _pub = create_publisher(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::SharedPtr _pub; +}; + +} // namespace component_manager +} // namespace env_manager + +#endif // ENV_MANAGER__COMPONENT_MANAGER__PUBLISHER_COMPONENT_HPP_ diff --git a/env_manager/include/component_manager/service_component.hpp b/env_manager/include/component_manager/service_component.hpp new file mode 100644 index 0000000..86bac7c --- /dev/null +++ b/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 + +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 +class ServiceComponent: public rclcpp::Node +{ +public: + explicit ServiceComponent(const rclcpp::NodeOptions& options) + : Node(DEFAULT_SERVICE_NDOE_NAME, options) + { + _service = create_service( + DEFAULT_SERVICE_NAME, std::bind( + &ServiceComponent::callback, this, + std::placeholders::_1, std::placeholders::_2)); + } + + virtual void callback(std::shared_ptr request, + std::shared_ptr response) = 0; + +private: + typename rclcpp::Service::SharedPtr _service; +}; + +} // namespace component_manager +} // namespace env_manager + +#endif // ENV_MANAGER__COMPONENT_MANAGER__SERVICE_COMPONENT_HPP_ diff --git a/env_manager/include/component_manager/subscriber_component.hpp b/env_manager/include/component_manager/subscriber_component.hpp new file mode 100644 index 0000000..80fb873 --- /dev/null +++ b/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 +#include + +#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 +class SubscriberComponent : public rclcpp::Node +{ +public: + explicit SubscriberComponent(const rclcpp::NodeOptions& options) + : Node(DEFAULT_SUB_NODE_NAME, options) + { + _sub = create_subscription( + 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::SharedPtr _sub; +}; + +} // namespace component_manager +} // namespace env_manager + +#endif // ENV_MANAGER_COMPONENT_MANAGER_SUBSCRIBER_COMPONENT_HPP_ diff --git a/env_manager/include/component_manager/visibility_control.h b/env_manager/include/component_manager/visibility_control.h new file mode 100644 index 0000000..e26be49 --- /dev/null +++ b/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_ diff --git a/env_manager/include/config/config.hpp.cmake b/env_manager/include/config/config.hpp.cmake new file mode 100644 index 0000000..708944f --- /dev/null +++ b/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_ diff --git a/env_manager/include/config/config_file_resolver.hpp b/env_manager/include/config/config_file_resolver.hpp new file mode 100644 index 0000000..28848d0 --- /dev/null +++ b/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& cfg_files_dirs); + + std::string GetPath(const std::string& basename) const; + std::string GetContent(const std::string& basename) const; + +private: + std::vector _config_files_dirs; + +}; + +} // namespace config +} // namespace env_manager + +#endif // ENV_MANAGER_CONFIG_CONFIG_FILE_RESOLVER_H_ diff --git a/env_manager/include/config/lua_file_resolver.hpp b/env_manager/include/config/lua_file_resolver.hpp new file mode 100644 index 0000000..375ccd5 --- /dev/null +++ b/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 +#include +#include + +} +#include +#include +#include +#include + +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 file_resolver); + + LuaParameterDictionary(const LuaParameterDictionary&) = delete; + LuaParameterDictionary& operator=(const LuaParameterDictionary&) = delete; + + // Constructs a LuaParameterDictionary without reference counting. + static std::unique_ptr NonReferenceCounted( + const std::string& code, std::unique_ptr file_resolver); + + ~LuaParameterDictionary(); + + // Returns all available keys. + std::vector 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 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 GetArrayValuesAsDoubles(); + std::vector GetArrayValuesAsStrings(); + std::vector> + GetArrayValuesAsDictionaries(); + + private: + enum class ReferenceCount { YES, NO }; + LuaParameterDictionary(const std::string& code, + ReferenceCount reference_count, + std::unique_ptr file_resolver); + + // For GetDictionary(). + LuaParameterDictionary(lua_State* L, ReferenceCount reference_count, + std::shared_ptr 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 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 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 reference_counts_; + + // List of all included files in order of inclusion. Used to prevent double + // inclusion. + std::vector included_files_; +}; +} // namespce config +} // namespace env_manager + +#endif // ENV_MANAGER_CONFIG_LUA_FILE_RESOLVER_H_ diff --git a/env_manager/include/config/options.hpp b/env_manager/include/config/options.hpp new file mode 100644 index 0000000..a1460e2 --- /dev/null +++ b/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 + +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 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 node_type_remap; +}; + +struct EnvManagerOption +{ + static EnvManagerOption create_option( + LuaParameterDictionary* lua_parameter_dictionary); + std::map environments; + std::map nodes; +}; + +} +} +#endif // ENV_MANAGER_CONFIG_OPTIONS_HPP_ diff --git a/env_manager/include/env_manager/environment_base.hpp b/env_manager/include/env_manager/environment_base.hpp new file mode 100644 index 0000000..e69de29 diff --git a/env_manager/include/env_manager/environment_manager.hpp b/env_manager/include/env_manager/environment_manager.hpp new file mode 100644 index 0000000..e69de29 diff --git a/env_manager/include/env_manager/environment_node_manager.hpp b/env_manager/include/env_manager/environment_node_manager.hpp new file mode 100644 index 0000000..cb5bca6 --- /dev/null +++ b/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 + +#include +#include + +extern "C" +{ +#include +#include +} + +namespace env_manager +{ + +class EnvironmentNodeManager +{ +public: + EnvironmentNodeManager(); + + ~EnvironmentNodeManager(); +}; + +} // namespace env_manager + +#endif // ENV_MANAGER__ENVIRONMENT_NODE_MANAGER_HPP_ diff --git a/env_manager/package.xml b/env_manager/package.xml new file mode 100644 index 0000000..b3fedd1 --- /dev/null +++ b/env_manager/package.xml @@ -0,0 +1,21 @@ + + + + env_manager + 0.0.0 + TODO: Package description + splinter1984 + TODO: License declaration + + ament_cmake_ros + + boost + rcl + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/env_manager/src/component_manager/component_manager.cpp b/env_manager/src/component_manager/component_manager.cpp new file mode 100644 index 0000000..c8983c9 --- /dev/null +++ b/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 + +namespace env_manager +{ +namespace component_manager +{ + +ComponentManager::ComponentManager(std::weak_ptr 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 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 &comps, + const std::map &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(); + 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(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(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 diff --git a/env_manager/src/config/config_file_resolver.cpp b/env_manager/src/config/config_file_resolver.cpp new file mode 100644 index 0000000..f945deb --- /dev/null +++ b/env_manager/src/config/config_file_resolver.cpp @@ -0,0 +1,51 @@ +#include "config/config_file_resolver.hpp" + +#include +#include +#include + +#include "config/config.hpp" + +#include + +namespace env_manager +{ +namespace config +{ + +ConfigurationFileResolver::ConfigurationFileResolver( + const std::vector& 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(stream), + std::istreambuf_iterator()); +} + +} // namespace config +} // namespace env_manager diff --git a/env_manager/src/config/lua_file_resolver.cpp b/env_manager/src/config/lua_file_resolver.cpp new file mode 100644 index 0000000..71ec231 --- /dev/null +++ b/env_manager/src/config/lua_file_resolver.cpp @@ -0,0 +1,446 @@ +#include "config/lua_file_resolver.hpp" +#include "glog/logging.h" + +#include +#include +#include +#include + +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 + lua_getfield(L, -1, + "format"); // S: ... string globals format + lua_pushstring(L, "%q"); // S: ... string globals format "%q" + lua_pushvalue(L, current_index); // S: ... string globals + // format "%q" string + + lua_call(L, 2, 1); // S: ... string globals quoted + lua_replace(L, current_index); // S: ... quoted globals + + 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(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 +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 +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& 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::NonReferenceCounted( + const std::string& code, std::unique_ptr file_resolver) { + return std::unique_ptr(new LuaParameterDictionary( + code, ReferenceCount::NO, std::move(file_resolver))); +} + +LuaParameterDictionary::LuaParameterDictionary( + const std::string& code, std::unique_ptr file_resolver) + : LuaParameterDictionary(code, ReferenceCount::YES, + std::move(file_resolver)) {} + +LuaParameterDictionary::LuaParameterDictionary( + const std::string& code, ReferenceCount reference_count, + std::unique_ptr 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 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 LuaParameterDictionary::GetKeys() const { + CheckTableIsAtTopOfStack(L_); + std::vector 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::GetDictionary( + const std::string& key) { + CheckHasKeyAndReference(key); + GetValueFromLuaTable(L_, key); + return PopDictionary(reference_count_); +} + +std::unique_ptr LuaParameterDictionary::PopDictionary( + ReferenceCount reference_count) const { + CheckTableIsAtTopOfStack(L_); + std::unique_ptr 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 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 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 LuaParameterDictionary::GetArrayValuesAsDoubles() { + std::vector values; + GetArrayValues(L_, [&values, this] { values.push_back(PopDouble()); }); + return values; +} + +std::vector> +LuaParameterDictionary::GetArrayValuesAsDictionaries() { + std::vector> values; + GetArrayValues(L_, [&values, this] { + values.push_back(PopDictionary(reference_count_)); + }); + return values; +} + +std::vector LuaParameterDictionary::GetArrayValuesAsStrings() { + std::vector 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 diff --git a/env_manager/src/config/options.cpp b/env_manager/src/config/options.cpp new file mode 100644 index 0000000..d6e2d94 --- /dev/null +++ b/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 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 +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 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 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 diff --git a/env_manager/src/env_manager/environment_node_manager.cpp b/env_manager/src/env_manager/environment_node_manager.cpp new file mode 100644 index 0000000..7b95a3e --- /dev/null +++ b/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 diff --git a/env_manager/src/main.cpp b/env_manager/src/main.cpp new file mode 100644 index 0000000..88f2456 --- /dev/null +++ b/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(); + + std::vector dirs = {}; + auto cfg_resolver = + ::std::make_unique<::env_manager::config::ConfigurationFileResolver>( + std::vector{ + 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 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; +} diff --git a/rbs_simulation/package.xml b/rbs_simulation/package.xml index bb4de1b..414d66b 100644 --- a/rbs_simulation/package.xml +++ b/rbs_simulation/package.xml @@ -14,7 +14,6 @@ tf2_ros std_msgs geometry_msgs - nlohmann_json ament_lint_auto ament_lint_common