Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Working async controllers and components [not synchronized] (backport #1041) #1528

Closed
wants to merge 1 commit into from
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
111 changes: 111 additions & 0 deletions controller_interface/include/controller_interface/async_controller.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,111 @@
// Copyright 2024 ros2_control development team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_
#define CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_

#include <atomic>
#include <memory>
#include <thread>

#include "controller_interface_base.hpp"
#include "lifecycle_msgs/msg/state.hpp"

namespace controller_interface
{

class AsyncControllerThread
{
public:
/// Constructor for the AsyncControllerThread object.
/**
*
* \param[in] controller shared pointer to a controller.
* \param[in] cm_update_rate the controller manager's update rate.
*/
AsyncControllerThread(
std::shared_ptr<controller_interface::ControllerInterfaceBase> & controller, int cm_update_rate)
: terminated_(false), controller_(controller), thread_{}, cm_update_rate_(cm_update_rate)
{
}

AsyncControllerThread(const AsyncControllerThread & t) = delete;
AsyncControllerThread(AsyncControllerThread && t) = delete;

// Destructor, called when the component is erased from its map.
~AsyncControllerThread()
{
terminated_.store(true, std::memory_order_seq_cst);
if (thread_.joinable())
{
thread_.join();
}
}

/// Creates the controller's thread.
/**
* Called when the controller is activated.
*
*/
void activate()
{
thread_ = std::thread(&AsyncControllerThread::controller_update_callback, this);
}

/// Periodically execute the controller's update method.
/**
* Callback of the async controller's thread.
* **Not synchronized with the controller manager's write and read currently**
*
*/
void controller_update_callback()
{
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();

auto previous_time = controller_->get_node()->now();
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)
{
auto const current_time = controller_->get_node()->now();
auto const measured_period = current_time - previous_time;
previous_time = current_time;
controller_->update(
controller_->get_node()->now(),
(controller_->get_update_rate() != cm_update_rate_ && controller_->get_update_rate() != 0)
? rclcpp::Duration::from_seconds(1.0 / controller_->get_update_rate())
: measured_period);
}

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_;
};

} // namespace controller_interface

#endif // CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include <utility>
#include <vector>

#include "controller_interface/async_controller.hpp"
#include "controller_interface/chainable_controller_interface.hpp"
#include "controller_interface/controller_interface.hpp"
#include "controller_interface/controller_interface_base.hpp"
Expand Down Expand Up @@ -194,6 +195,15 @@ class ControllerManager : public rclcpp::Node
CONTROLLER_MANAGER_PUBLIC
unsigned int get_update_rate() const;

/// Deletes all async controllers and components.
/**
* Needed to join the threads immediately after the control loop is ended
* to avoid unnecessary iterations. Otherwise
* the threads will be joined only when the controller manager gets destroyed.
*/
CONTROLLER_MANAGER_PUBLIC
void shutdown_async_controllers_and_components();

protected:
CONTROLLER_MANAGER_PUBLIC
void init_services();
Expand Down Expand Up @@ -520,6 +530,12 @@ class ControllerManager : public rclcpp::Node
};

SwitchParams switch_params_;
<<<<<<< HEAD
=======

std::unordered_map<std::string, std::unique_ptr<controller_interface::AsyncControllerThread>>
async_controller_threads_;
>>>>>>> 2cbe470 (Working async controllers and components [not synchronized] (#1041))
};

} // namespace controller_manager
Expand Down
25 changes: 25 additions & 0 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -744,6 +744,24 @@ controller_interface::return_type ControllerManager::configure_controller(
controller_name.c_str(), new_state.label().c_str());
return controller_interface::return_type::ERROR;
}
<<<<<<< HEAD
=======
catch (...)
{
RCLCPP_ERROR(
get_logger(), "Caught unknown exception while configuring controller '%s'",
controller_name.c_str());
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_interface::AsyncControllerThread>(controller, update_rate_));
}
>>>>>>> 2cbe470 (Working async controllers and components [not synchronized] (#1041))

const auto controller_update_rate = controller->get_update_rate();
const auto cm_update_rate = get_update_rate();
Expand Down Expand Up @@ -2135,6 +2153,13 @@ std::pair<std::string, std::string> ControllerManager::split_command_interface(

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

void ControllerManager::shutdown_async_controllers_and_components()
{
async_controller_threads_.erase(
async_controller_threads_.begin(), async_controller_threads_.end());
resource_manager_->shutdown_async_components();
}

void ControllerManager::propagate_deactivation_of_chained_mode(
const std::vector<ControllerSpec> & controllers)
{
Expand Down
2 changes: 2 additions & 0 deletions controller_manager/src/ros2_control_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,8 @@ int main(int argc, char ** argv)
next_iteration_time += period;
std::this_thread::sleep_until(next_iteration_time);
}

cm->shutdown_async_controllers_and_components();
});

executor->add_node(cm);
Expand Down
122 changes: 122 additions & 0 deletions hardware_interface/include/hardware_interface/async_components.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,122 @@
// Copyright 2023 ros2_control development team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_
#define HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_

#include <atomic>
#include <thread>
#include <type_traits>
#include <variant>

#include "hardware_interface/actuator.hpp"
#include "hardware_interface/sensor.hpp"
#include "hardware_interface/system.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/time.hpp"

namespace hardware_interface
{

class AsyncComponentThread
{
public:
explicit AsyncComponentThread(
unsigned int update_rate,
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
: cm_update_rate_(update_rate), clock_interface_(clock_interface)
{
}

// Fills the internal variant with the desired component.
template <typename T>
void register_component(T * component)
{
hardware_component_ = component;
}

AsyncComponentThread(const AsyncComponentThread & t) = delete;
AsyncComponentThread(AsyncComponentThread && t) = delete;

// Destructor, called when the component is erased from its map.
~AsyncComponentThread()
{
terminated_.store(true, std::memory_order_seq_cst);
if (write_and_read_.joinable())
{
write_and_read_.join();
}
}
/// Creates the component's thread.
/**
* Called when the component is activated.
*
*/
void activate() { write_and_read_ = std::thread(&AsyncComponentThread::write_and_read, this); }

/// Periodically execute the component's write and read methods.
/**
* Callback of the async component's thread.
* **Not synchronized with the controller manager's update currently**
*
*/
void write_and_read()
{
using TimePoint = std::chrono::system_clock::time_point;

std::visit(
[this](auto & component)
{
auto previous_time = clock_interface_->get_clock()->now();
while (!terminated_.load(std::memory_order_relaxed))
{
auto const period = std::chrono::nanoseconds(1'000'000'000 / cm_update_rate_);
TimePoint next_iteration_time =
TimePoint(std::chrono::nanoseconds(clock_interface_->get_clock()->now().nanoseconds()));

if (component->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
auto current_time = clock_interface_->get_clock()->now();
auto measured_period = current_time - previous_time;
previous_time = current_time;

if (!first_iteration)
{
component->write(clock_interface_->get_clock()->now(), measured_period);
}
component->read(clock_interface_->get_clock()->now(), measured_period);
first_iteration = false;
}
next_iteration_time += period;
std::this_thread::sleep_until(next_iteration_time);
}
},
hardware_component_);
}

private:
std::atomic<bool> terminated_{false};
std::variant<Actuator *, System *, Sensor *> hardware_component_;
std::thread write_and_read_{};

unsigned int cm_update_rate_;
bool first_iteration = true;
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
};

}; // namespace hardware_interface

#endif // HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -369,6 +369,12 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
return_type set_component_state(
const std::string & component_name, rclcpp_lifecycle::State & target_state);

/// Deletes all async components from the resource storage
/**
* Needed to join the threads immediately after the control loop is ended.
*/
void shutdown_async_components();

/// Reads all loaded hardware components.
/**
* Reads from all active hardware components.
Expand Down
19 changes: 19 additions & 0 deletions hardware_interface/src/resource_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -193,6 +193,18 @@ class ResourceStorage
hardware.get_name().c_str(), interface.c_str());
}
}
<<<<<<< HEAD
=======

if (hardware_info_map_[hardware.get_name()].is_async)
{
async_component_threads_.emplace(
std::piecewise_construct, std::forward_as_tuple(hardware.get_name()),
std::forward_as_tuple(cm_update_rate_, clock_interface_));

async_component_threads_.at(hardware.get_name()).register_component(&hardware);
}
>>>>>>> 2cbe470 (Working async controllers and components [not synchronized] (#1041))
}
return result;
}
Expand Down Expand Up @@ -1228,6 +1240,13 @@ return_type ResourceManager::set_component_state(
return result;
}

void ResourceManager::shutdown_async_components()
{
resource_storage_->async_component_threads_.erase(
resource_storage_->async_component_threads_.begin(),
resource_storage_->async_component_threads_.end());
}

// CM API: Called in "update"-thread
HardwareReadWriteStatus ResourceManager::read(
const rclcpp::Time & time, const rclcpp::Duration & period)
Expand Down
Loading