robossembler/runtime
7
0
Fork 0
Code Issues 4 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,
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());
RCLCPP_INFO(get_logger(), "Unloading existing environment '%s'",
request->name.c_str());
unloadEnv(request->name);
}
auto loadedEnv = loadEnv(request->name, request->type);
auto configuredEnv = configureEnv(request->name);
response->ok = (loadedEnv.get() != nullptr &&
configureEnv(request->name) == EnvStateReturnType::SUCCESS)
configuredEnv == 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) {
std::lock_guard<std::mutex> lock(m_env_mutex);
response->ok = loadEnv(request->name, request->type).get() != nullptr;
}
void EnvManager::configureEnv_cb(
const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request,
env_manager_interfaces::srv::ConfigureEnv::Response::SharedPtr response) {
std::lock_guard<std::mutex> guard(m_env_mutex);
response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS;
}
void EnvManager::unloadEnv_cb(
const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) {
std::lock_guard<std::mutex> lock(m_env_mutex);
response->ok = unloadEnv(request->name) == EnvStateReturnType::SUCCESS;
}
EnvInterfaceSharedPtr EnvManager::addEnv(const EnvSpec &enviroment) {
std::lock_guard<std::mutex> lock(m_env_mutex);
// generate list of active enviroments
// std::string unique_env_name = enviroment.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) {
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(
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 &) {
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();
m_tf2_broadcaster_target.reset();
// Clear other variables
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;
}