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

env_manager: clear variables on cleanup, remove mutex

Browse source
This commit is contained in:
Ilya Uraev 2023-12-19 11:11:42 +03:00
parent 6d81f59204
commit d7d3950052
2 changed files with 16 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

14
env_manager/env_manager/src/env_manager.cpp
View file

@ -54,46 +54,42 @@ void EnvManager::startEnv_cb(
const env_manager_interfaces::srv::StartEnv::Request::SharedPtr request, const env_manager_interfaces::srv::StartEnv::Request::SharedPtr request,
env_manager_interfaces::srv::StartEnv::Response::SharedPtr response) { env_manager_interfaces::srv::StartEnv::Response::SharedPtr response) {
std::lock_guard<std::mutex> lock(m_env_mutex);
auto it = std::find_if( auto it = std::find_if(
m_active_envs.begin(), m_active_envs.end(), m_active_envs.begin(), m_active_envs.end(),
[&request](const EnvSpec &env) { return env.name == request->name; }); [&request](const EnvSpec &env) { return env.name == request->name; });
if (it != m_active_envs.end()) { if (it != m_active_envs.end()) {
RCLCPP_INFO(get_logger(), "Unloading existing environment '%s'", request->name.c_str()); RCLCPP_INFO(get_logger(), "Unloading existing environment '%s'",
request->name.c_str());
unloadEnv(request->name); unloadEnv(request->name);
} }
auto loadedEnv = loadEnv(request->name, request->type); auto loadedEnv = loadEnv(request->name, request->type);
auto configuredEnv = configureEnv(request->name);
response->ok = (loadedEnv.get() != nullptr && response->ok = (loadedEnv.get() != nullptr &&
configureEnv(request->name) == EnvStateReturnType::SUCCESS) configuredEnv == EnvStateReturnType::SUCCESS)
? true ? true
: false; : 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) {
std::lock_guard<std::mutex> lock(m_env_mutex);
response->ok = loadEnv(request->name, request->type).get() != nullptr; response->ok = loadEnv(request->name, request->type).get() != nullptr;
} }
void EnvManager::configureEnv_cb( void EnvManager::configureEnv_cb(
const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request, const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request,
env_manager_interfaces::srv::ConfigureEnv::Response::SharedPtr response) { env_manager_interfaces::srv::ConfigureEnv::Response::SharedPtr response) {
std::lock_guard<std::mutex> guard(m_env_mutex);
response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS; response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS;
} }
void EnvManager::unloadEnv_cb( void EnvManager::unloadEnv_cb(
const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request, const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) { env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) {
std::lock_guard<std::mutex> lock(m_env_mutex);
response->ok = unloadEnv(request->name) == EnvStateReturnType::SUCCESS; response->ok = unloadEnv(request->name) == EnvStateReturnType::SUCCESS;
} }
EnvInterfaceSharedPtr EnvManager::addEnv(const EnvSpec &enviroment) { EnvInterfaceSharedPtr EnvManager::addEnv(const EnvSpec &enviroment) {
std::lock_guard<std::mutex> lock(m_env_mutex);
// generate list of active enviroments // generate list of active enviroments
// std::string unique_env_name = enviroment.name;// + "_" + // std::string unique_env_name = enviroment.name;// + "_" +
// std::to_string(m_active_envs.size()); m_active_envs[unique_env_name] = // std::to_string(m_active_envs.size()); m_active_envs[unique_env_name] =
@ -174,7 +170,7 @@ EnvStateReturnType EnvManager::unloadEnv(const std::string &env_name) {
// } // }
EnvStateReturnType EnvManager::configureEnv(const std::string &env_name) { EnvStateReturnType EnvManager::configureEnv(const std::string &env_name) {
std::lock_guard<std::mutex> guard(m_env_mutex); RCLCPP_INFO(this->get_logger(), "Configuring enviroment '%s'", env_name.c_str());
auto it = std::find_if( auto it = std::find_if(
m_active_envs.begin(), m_active_envs.end(), m_active_envs.begin(), m_active_envs.end(),

13
env_manager/gz_enviroment/src/gz_enviroment.cpp
View file

@ -67,10 +67,19 @@ CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State &) {
CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State &) { CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State &) {
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup"); RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup");
// Clear tf2 broadcasters
// m_tf2_broadcaster.reset();
// m_tf2_broadcaster_target.reset();
m_tf2_broadcaster.reset(); // Clear other variables
m_tf2_broadcaster_target.reset();
m_gz_node.reset(); m_gz_node.reset();
m_follow_frames.clear();
m_topic_name.clear();
m_service_spawn.clear();
m_world_name.clear();
m_config_loader.reset();
m_transforms = tf2_msgs::msg::TFMessage();
m_target_places = tf2_msgs::msg::TFMessage();
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }