diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 6b4d556c48..926449e201 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -130,4 +130,6 @@ std::shared_ptr ControllerInterfaceBase::get_no unsigned int ControllerInterfaceBase::get_update_rate() const { return update_rate_; } +bool ControllerInterfaceBase::is_async() const { return is_async_; } + } // namespace controller_interface diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index a7ec2029d1..537f0447be 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -514,6 +514,67 @@ class ControllerManager : public rclcpp::Node }; SwitchParams switch_params_; + + class ControllerThreadWrapper + { + public: + ControllerThreadWrapper( + std::shared_ptr & controller, + int cm_update_rate) + : terminated_(false), controller_(controller), thread_{}, cm_update_rate_(cm_update_rate) + { + } + + ControllerThreadWrapper(const ControllerThreadWrapper & t) = delete; + ControllerThreadWrapper(ControllerThreadWrapper && t) = default; + ~ControllerThreadWrapper() + { + terminated_.store(true, std::memory_order_seq_cst); + if (thread_.joinable()) + { + thread_.join(); + } + } + + void activate() + { + thread_ = std::thread(&ControllerThreadWrapper::call_controller_update, this); + } + + void call_controller_update() + { + using TimePoint = std::chrono::system_clock::time_point; + unsigned int used_update_rate = + controller_->get_update_rate() == 0 + ? cm_update_rate_ + : controller_ + ->get_update_rate(); // determines if the controller's or CM's update rate is used + + while (!terminated_.load(std::memory_order_relaxed)) + { + auto const period = std::chrono::nanoseconds(1'000'000'000 / used_update_rate); + TimePoint next_iteration_time = + TimePoint(std::chrono::nanoseconds(controller_->get_node()->now().nanoseconds())); + + if (controller_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + // critical section, not implemented yet + } + + next_iteration_time += period; + std::this_thread::sleep_until(next_iteration_time); + } + } + + private: + std::atomic terminated_; + std::shared_ptr controller_; + std::thread thread_; + unsigned int cm_update_rate_; + }; + + std::unordered_map> + async_controller_threads_; }; } // namespace controller_manager diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 88f913bc72..b912b622e2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -403,6 +403,13 @@ controller_interface::return_type ControllerManager::unload_controller( controller_name.c_str()); return controller_interface::return_type::ERROR; } + if (controller.c->is_async()) + { + RCLCPP_DEBUG( + get_logger(), "Removing controller '%s' from the list of async controllers", + controller_name.c_str()); + async_controller_threads_.erase(controller_name); + } RCLCPP_DEBUG(get_logger(), "Cleanup controller"); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for @@ -420,6 +427,7 @@ controller_interface::return_type ControllerManager::unload_controller( RCLCPP_DEBUG(get_logger(), "Destruct controller finished"); RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str()); + return controller_interface::return_type::OK; } @@ -487,6 +495,13 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } + // ASYNCHRONOUS CONTROLLERS: Start background thread for update + if (controller->is_async()) + { + async_controller_threads_.emplace( + controller_name, std::make_unique(controller, update_rate_)); + } + // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers if (controller->is_chainable()) { @@ -1178,6 +1193,7 @@ void ControllerManager::activate_controllers( continue; } auto controller = found_it->c; + auto controller_name = found_it->info.name; bool assignment_successful = true; // assign command interfaces to the controller @@ -1266,6 +1282,11 @@ void ControllerManager::activate_controllers( get_logger(), "After activating, controller '%s' is in state '%s', expected Active", controller->get_node()->get_name(), new_state.label().c_str()); } + + if (controller->is_async()) + { + async_controller_threads_.at(controller_name)->activate(); + } } // All controllers activated, switching done switch_params_.do_switch = false; @@ -1740,7 +1761,7 @@ controller_interface::return_type ControllerManager::update( { // TODO(v-lopez) we could cache this information // https://github.com/ros-controls/ros2_control/issues/153 - if (is_controller_active(*loaded_controller.c)) + if (!loaded_controller.c->is_async() && is_controller_active(*loaded_controller.c)) { const auto controller_update_rate = loaded_controller.c->get_update_rate();