diff --git a/env_manager/env_manager/src/env_manager.cpp b/env_manager/env_manager/src/env_manager.cpp index ca53bd9..e6755bf 100644 --- a/env_manager/env_manager/src/env_manager.cpp +++ b/env_manager/env_manager/src/env_manager.cpp @@ -1,5 +1,9 @@ #include "env_manager/env_manager.hpp" #include "nlohmann/json.hpp" +#include +#include +#include +#include #include static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAMESPACE = @@ -44,10 +48,33 @@ void EnvManager::initServices() { std::bind(&EnvManager::configureEnv_cb, this, _1, _2)); m_unload_env_srv = create_service( "~/unload_env", std::bind(&EnvManager::unloadEnv_cb, this, _1, _2)); + m_start_env_srv = create_service( + "~/start_env", std::bind(&EnvManager::startEnv_cb, this, _1, _2)); } rclcpp::Node::SharedPtr EnvManager::getNode() { return m_node; } +void EnvManager::startEnv_cb( + const env_manager_interfaces::srv::StartEnv::Request::SharedPtr request, + env_manager_interfaces::srv::StartEnv::Response::SharedPtr response) { + + std::lock_guard lock(m_env_mutex); + + auto it = std::find_if( + m_active_envs.begin(), m_active_envs.end(), + [&request](const EnvSpec &env) { return env.name == request->name; }); + + if (it != m_active_envs.end()) { + RCLCPP_INFO(get_logger(), "Unloading existing environment '%s'", request->name.c_str()); + unloadEnv(request->name); + } + + auto loadedEnv = loadEnv(request->name, request->type); + response->ok = (loadedEnv.get() != nullptr && + configureEnv(request->name) == EnvStateReturnType::SUCCESS) + ? true + : false; +} void EnvManager::loadEnv_cb( const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request, env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response) { @@ -128,6 +155,7 @@ EnvStateReturnType EnvManager::unloadEnv(const std::string &env_name) { [&env_name](const EnvSpec &env) { return env.name == env_name; }); if (it != m_active_envs.end()) { + it->env_ptr->getNode()->deactivate(); it->env_ptr->getNode()->cleanup(); m_executor->remove_node(it->env_ptr->getNode()->get_node_base_interface()); m_active_envs.erase(it);