diff --git a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp index 13eb8eba6..ebcc5aa8f 100644 --- a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp +++ b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp @@ -401,7 +401,11 @@ template void NodeCanopen402Driver::deactivate(bool called_from_base) { NodeCanopenProxyDriver::deactivate(false); - timer_->cancel(); + if(timer_ != nullptr){ + timer_->cancel(); + } + this->exec_->post([this]() { motor_.reset(); }); + } template diff --git a/canopen_core/src/lifecycle_manager.cpp b/canopen_core/src/lifecycle_manager.cpp index 701cdb6bf..d29317505 100644 --- a/canopen_core/src/lifecycle_manager.cpp +++ b/canopen_core/src/lifecycle_manager.cpp @@ -78,8 +78,11 @@ bool LifecycleManager::load_from_config() for (auto it = devices.begin(); it != devices.end(); it++) { uint8_t node_id = config_->get_entry(*it, "node_id").value(); - std::string change_state_client_name = *it; - std::string get_state_client_name = *it; + std::string device_namespace = config_->get_entry(*it, "namespace").value(); + device_namespace += "/"; + device_namespace += *it; + std::string change_state_client_name = device_namespace; + std::string get_state_client_name = device_namespace; get_state_client_name += "/get_state"; change_state_client_name += "/change_state"; RCLCPP_INFO(this->get_logger(), "Found %s (node_id=%hu)", it->c_str(), node_id);