100 lines
3.9 KiB
C++
100 lines
3.9 KiB
C++
#ifndef __ENV_MANAGER_HPP__
|
|
#define __ENV_MANAGER_HPP__
|
|
|
|
#include "env_manager_interfaces/srv/configure_env.hpp"
|
|
#include "env_manager_interfaces/srv/load_env.hpp"
|
|
#include "env_manager_interfaces/srv/unload_env.hpp"
|
|
#include "env_manager_interfaces/srv/start_env.hpp"
|
|
#include "rclcpp/rclcpp.hpp"
|
|
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
|
|
|
#include "env_interface/env_interface.hpp"
|
|
|
|
#include "pluginlib/class_loader.hpp"
|
|
|
|
namespace env_manager {
|
|
using EnvInterface = env_interface::EnvInterface;
|
|
using EnvInterfaceSharedPtr = std::shared_ptr<env_interface::EnvInterface>;
|
|
using EnvStateReturnType =
|
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
|
|
|
|
struct EnvSpec {
|
|
std::string name;
|
|
std::string type;
|
|
EnvInterfaceSharedPtr env_ptr;
|
|
};
|
|
|
|
class EnvManager : public rclcpp::Node {
|
|
public:
|
|
// EnvManager(rclcpp::Executor::SharedPtr executor,
|
|
// const std::string &asm_config,
|
|
// const std::string &node_name = "env_manager",
|
|
// const std::string &node_namespace = "",
|
|
// rclcpp::NodeOptions &node_options =
|
|
// rclcpp::NodeOptions()
|
|
// .allow_undeclared_parameters(true)
|
|
// .automatically_declare_parameters_from_overrides(true));
|
|
EnvManager(rclcpp::Executor::SharedPtr executor,
|
|
const std::string &node_name = "env_manager",
|
|
const std::string &node_namespace = "",
|
|
rclcpp::NodeOptions &node_options =
|
|
rclcpp::NodeOptions()
|
|
.allow_undeclared_parameters(true)
|
|
.automatically_declare_parameters_from_overrides(true));
|
|
virtual ~EnvManager() = default;
|
|
|
|
// EnvInterfaceSharedPtr loadEnv(const std::string& env_name);
|
|
EnvInterfaceSharedPtr loadEnv(const std::string &env_name,
|
|
const std::string &env_class_type);
|
|
EnvStateReturnType unloadEnv(const std::string &env_name);
|
|
EnvStateReturnType configureEnv(const std::string &env_name);
|
|
EnvInterfaceSharedPtr addEnv(const EnvSpec &enviment);
|
|
|
|
// TODO: Define the input data format
|
|
// Set Target for RL enviroment
|
|
EnvInterfaceSharedPtr setTarget();
|
|
EnvInterfaceSharedPtr switchTarget();
|
|
EnvInterfaceSharedPtr unsetTarget();
|
|
// Load Constraints for RL enviroment
|
|
EnvInterfaceSharedPtr loadConstraints();
|
|
EnvInterfaceSharedPtr switchConstraints();
|
|
EnvInterfaceSharedPtr unloadConstraints();
|
|
|
|
protected:
|
|
void initServices();
|
|
rclcpp::Node::SharedPtr getNode();
|
|
void startEnv_cb(
|
|
const env_manager_interfaces::srv::StartEnv::Request::SharedPtr request,
|
|
env_manager_interfaces::srv::StartEnv::Response::SharedPtr response);
|
|
void loadEnv_cb(
|
|
const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request,
|
|
env_manager_interfaces::srv::LoadEnv_Response::SharedPtr response);
|
|
|
|
void configureEnv_cb(
|
|
const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr
|
|
request,
|
|
env_manager_interfaces::srv::ConfigureEnv_Response::SharedPtr response);
|
|
|
|
void unloadEnv_cb(
|
|
const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request,
|
|
env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response);
|
|
|
|
private:
|
|
rclcpp::Node::SharedPtr m_node;
|
|
std::mutex m_env_mutex;
|
|
std::vector<EnvSpec> m_active_envs;
|
|
rclcpp::Service<env_manager_interfaces::srv::LoadEnv>::SharedPtr
|
|
m_load_env_srv;
|
|
rclcpp::Service<env_manager_interfaces::srv::ConfigureEnv>::SharedPtr
|
|
m_configure_env_srv;
|
|
rclcpp::Service<env_manager_interfaces::srv::UnloadEnv>::SharedPtr
|
|
m_unload_env_srv;
|
|
rclcpp::Service<env_manager_interfaces::srv::StartEnv>::SharedPtr m_start_env_srv;
|
|
|
|
rclcpp::CallbackGroup::SharedPtr m_callback_group;
|
|
rclcpp::Executor::SharedPtr m_executor;
|
|
|
|
std::shared_ptr<pluginlib::ClassLoader<env_interface::EnvInterface>> m_loader;
|
|
};
|
|
} // namespace env_manager
|
|
#endif // __ENV_MANAGER_HPP__
|