Skip to content

Commit

Permalink
[ControllerManager] Add Class for Async Controllers and Lifecycle Man…
Browse files Browse the repository at this point in the history
…agement (#932)

* add `is_async` param to controller manager
* async controller lifecycle
* use controller's update rate

Co-authored-by: Bence Magyar <[email protected]>
  • Loading branch information
VX792 and bmagyar authored Mar 21, 2023
1 parent 7ed3786 commit 5dfacd8
Show file tree
Hide file tree
Showing 3 changed files with 85 additions and 1 deletion.
2 changes: 2 additions & 0 deletions controller_interface/src/controller_interface_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -130,4 +130,6 @@ std::shared_ptr<rclcpp_lifecycle::LifecycleNode> ControllerInterfaceBase::get_no

unsigned int ControllerInterfaceBase::get_update_rate() const { return update_rate_; }

bool ControllerInterfaceBase::is_async() const { return is_async_; }

} // namespace controller_interface
Original file line number Diff line number Diff line change
Expand Up @@ -514,6 +514,67 @@ class ControllerManager : public rclcpp::Node
};

SwitchParams switch_params_;

class ControllerThreadWrapper
{
public:
ControllerThreadWrapper(
std::shared_ptr<controller_interface::ControllerInterfaceBase> & 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<bool> terminated_;
std::shared_ptr<controller_interface::ControllerInterfaceBase> controller_;
std::thread thread_;
unsigned int cm_update_rate_;
};

std::unordered_map<std::string, std::unique_ptr<ControllerThreadWrapper>>
async_controller_threads_;
};

} // namespace controller_manager
Expand Down
23 changes: 22 additions & 1 deletion controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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;
}

Expand Down Expand Up @@ -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<ControllerThreadWrapper>(controller, update_rate_));
}

// CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers
if (controller->is_chainable())
{
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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();

Expand Down

0 comments on commit 5dfacd8

Please sign in to comment.