@@ -890,7 +890,7 @@ void ControllerManager::configure_controller_service_cb(
890890 RCLCPP_DEBUG (
891891 get_logger (), " configuring service called for controller '%s' " , request->name .c_str ());
892892 std::lock_guard<std::mutex> guard (services_lock_);
893- RCLCPP_DEBUG (get_logger (), " loading service locked" );
893+ RCLCPP_DEBUG (get_logger (), " configuring service locked" );
894894
895895 response->ok =
896896 configure_controller (request->name ) == controller_interface::return_type::OK;
@@ -1153,7 +1153,9 @@ ControllerManager::RTControllerListWrapper::update_and_get_used_by_rt_list()
11531153std::vector<ControllerSpec> & ControllerManager::RTControllerListWrapper::get_unused_list (
11541154 const std::lock_guard<std::recursive_mutex> &)
11551155{
1156- assert (controllers_lock_.try_lock ());
1156+ if (!controllers_lock_.try_lock ()) {
1157+ throw std::runtime_error (" controllers_lock_ not owned by thread" );
1158+ }
11571159 controllers_lock_.unlock ();
11581160 // Get the index to the outdated controller list
11591161 int free_controllers_list = get_other_list (updated_controllers_index_);
@@ -1166,15 +1168,19 @@ std::vector<ControllerSpec> & ControllerManager::RTControllerListWrapper::get_un
11661168const std::vector<ControllerSpec> & ControllerManager::RTControllerListWrapper::get_updated_list (
11671169 const std::lock_guard<std::recursive_mutex> &) const
11681170{
1169- assert (controllers_lock_.try_lock ());
1171+ if (!controllers_lock_.try_lock ()) {
1172+ throw std::runtime_error (" controllers_lock_ not owned by thread" );
1173+ }
11701174 controllers_lock_.unlock ();
11711175 return controllers_lists_[updated_controllers_index_];
11721176}
11731177
11741178void ControllerManager::RTControllerListWrapper::switch_updated_list (
11751179 const std::lock_guard<std::recursive_mutex> &)
11761180{
1177- assert (controllers_lock_.try_lock ());
1181+ if (!controllers_lock_.try_lock ()) {
1182+ throw std::runtime_error (" controllers_lock_ not owned by thread" );
1183+ }
11781184 controllers_lock_.unlock ();
11791185 int former_current_controllers_list_ = updated_controllers_index_;
11801186 updated_controllers_index_ = get_other_list (former_current_controllers_list_);
0 commit comments