robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

add cb for starter and fix unload func

Browse source
This commit is contained in:
Ilya Uraev 2023-12-19 08:12:49 +03:00
parent 09b4c79e55
commit 894c9c9b69
1 changed files with 28 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

28
env_manager/env_manager/src/env_manager.cpp
View file

@ -1,5 +1,9 @@
#include "env_manager/env_manager.hpp" #include "env_manager/env_manager.hpp"
#include "nlohmann/json.hpp" #include "nlohmann/json.hpp"
#include <cstddef>
#include <env_manager_interfaces/srv/detail/start_env__struct.hpp>
#include <functional>
#include <mutex>
#include <string> #include <string>
static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAMESPACE = static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAMESPACE =
@ -44,10 +48,33 @@ void EnvManager::initServices() {
std::bind(&EnvManager::configureEnv_cb, this, _1, _2)); std::bind(&EnvManager::configureEnv_cb, this, _1, _2));
m_unload_env_srv = create_service<env_manager_interfaces::srv::UnloadEnv>( m_unload_env_srv = create_service<env_manager_interfaces::srv::UnloadEnv>(
"~/unload_env", std::bind(&EnvManager::unloadEnv_cb, this, _1, _2)); "~/unload_env", std::bind(&EnvManager::unloadEnv_cb, this, _1, _2));
m_start_env_srv = create_service<env_manager_interfaces::srv::StartEnv>(
"~/start_env", std::bind(&EnvManager::startEnv_cb, this, _1, _2));
} }
rclcpp::Node::SharedPtr EnvManager::getNode() { return m_node; } 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<std::mutex> 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( void EnvManager::loadEnv_cb(
const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request, const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response) { 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; }); [&env_name](const EnvSpec &env) { return env.name == env_name; });
if (it != m_active_envs.end()) { if (it != m_active_envs.end()) {
it->env_ptr->getNode()->deactivate();
it->env_ptr->getNode()->cleanup(); it->env_ptr->getNode()->cleanup();
m_executor->remove_node(it->env_ptr->getNode()->get_node_base_interface()); m_executor->remove_node(it->env_ptr->getNode()->get_node_base_interface());
m_active_envs.erase(it); m_active_envs.erase(it);