Environment manager base architecture implemented

This commit is contained in:
Roman Andrianov 2023-02-08 15:06:48 +00:00 committed by Igor Brylyov
parent 2019e7db41
commit 9f27ad0af3
30 changed files with 1555 additions and 1 deletions

View file

@ -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

126
env_manager/CMakeLists.txt Normal file
View file

@ -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()

0
env_manager/README.md Normal file
View file

View file

@ -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

View file

@ -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"
}
}
}

View file

@ -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"
}
}
}

View file

@ -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"
}
}

View file

@ -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

View file

@ -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_

View file

@ -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_

View file

@ -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_

View file

@ -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_

View file

@ -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_

View file

@ -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_

View file

@ -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_

View file

@ -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_

View file

@ -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_

View file

@ -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_

View file

@ -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_

21
env_manager/package.xml Normal file
View file

@ -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>

View file

@ -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

View file

@ -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

View file

@ -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

View file

@ -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

View file

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

60
env_manager/src/main.cpp Normal file
View file

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

View file

@ -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>