diff --git a/env_manager/env_manager/include/env_manager/env_base_interface.hpp b/env_manager/env_manager/include/env_manager/env_base_interface.hpp index 355f0e5..e08e4a3 100644 --- a/env_manager/env_manager/include/env_manager/env_base_interface.hpp +++ b/env_manager/env_manager/include/env_manager/env_base_interface.hpp @@ -1,5 +1,5 @@ #include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" // TODO: BaseInterface class for next enviroments based on LifecycleNodeInterface // It's should be like ControllerInterfaceBase class from ros2_control @@ -9,16 +9,32 @@ namespace env_interface using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -class EnvBaseIntrerface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface +class EnvInterfaceBase : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface { public: - EnvBaseIntrerface() = default; + EnvInterfaceBase() = default; - virtual ~EnvBaseIntrerface() = default; + virtual ~EnvInterfaceBase() = default; + + virtual CallbackReturn init( + const std::string& t_env_name, + const std::string & t_namespace = "", + const rclcpp::NodeOptions & t_node_options = rclcpp::NodeOptions() + ); + + const rclcpp_lifecycle::State& configure(); virtual CallbackReturn on_init() = 0; - + std::shared_ptr getNode(); + std::shared_ptr getNode() const; + + const rclcpp_lifecycle::State & getState() const; + + + +private: + std::shared_ptr m_node; }; diff --git a/env_manager/env_manager/src/env_base_interface.cpp b/env_manager/env_manager/src/env_base_interface.cpp new file mode 100644 index 0000000..8b2dd68 --- /dev/null +++ b/env_manager/env_manager/src/env_base_interface.cpp @@ -0,0 +1,63 @@ +#include "env_manager/env_base_interface.hpp" + +namespace env_interface +{ + +CallbackReturn +EnvInterfaceBase::init( + const std::string& t_env_name, + const std::string & t_namespace, + const rclcpp::NodeOptions & t_node_options) +{ + m_node = std::make_shared( + t_env_name, t_namespace, t_node_options, false); + + if (on_init() == CallbackReturn::FAILURE) + { + return CallbackReturn::FAILURE; + } + + m_node->register_on_configure( + std::bind(&EnvInterfaceBase::on_configure, this, std::placeholders::_1) + ); + m_node->register_on_activate( + std::bind(&EnvInterfaceBase::on_activate, this, std::placeholders::_1) + ); + m_node->register_on_cleanup( + std::bind(&EnvInterfaceBase::on_cleanup, this, std::placeholders::_1) + ); + m_node->register_on_deactivate( + std::bind(&EnvInterfaceBase::on_deactivate, this, std::placeholders::_1) + ); + m_node->register_on_error( + std::bind(&EnvInterfaceBase::on_error, this, std::placeholders::_1) + ); + m_node->register_on_shutdown( + std::bind(&EnvInterfaceBase::on_shutdown, this, std::placeholders::_1) + ); + + return CallbackReturn::SUCCESS; +} + +const rclcpp_lifecycle::State& +EnvInterfaceBase::configure() +{ + return getNode()->configure(); +} + +std::shared_ptr +EnvInterfaceBase::getNode() +{ + if (!m_node.get()) + { + throw std::runtime_error("Lifecycle node hasn't been initialized yet"); + } + return m_node; +} + +const rclcpp_lifecycle::State & EnvInterfaceBase::getState() const +{ + return m_node->get_current_state(); +} + +} // namespace env_interface