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

Async Hardware Components #1567

Merged
Merged
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
Original file line number Diff line number Diff line change
Expand Up @@ -222,15 +222,6 @@ 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
saikishor marked this conversation as resolved.
Show resolved Hide resolved
void shutdown_async_controllers_and_components();

protected:
CONTROLLER_MANAGER_PUBLIC
void init_services();
Expand Down
5 changes: 0 additions & 5 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2692,11 +2692,6 @@ 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()
{
resource_manager_->shutdown_async_components();
}

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

cm->shutdown_async_controllers_and_components();
});

executor->add_node(cm);
Expand Down
1 change: 1 addition & 0 deletions doc/release_notes.rst
Original file line number Diff line number Diff line change
Expand Up @@ -157,6 +157,7 @@ hardware_interface
* With (`#1683 <https://github.com/ros-controls/ros2_control/pull/1683>`_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 <https://github.com/ros-controls/ros2_control/pull/1240>`_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles.
* With (`#1421 <https://github.com/ros-controls/ros2_control/pull/1421>`_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file.
* With (`#1763 <https://github.com/ros-controls/ros2_control/pull/1763>`_) parsing for SDF published to ``robot_description`` topic is now also supported.
* With (`#1567 <https://github.com/ros-controls/ros2_control/pull/1567>`_) all the Hardware components now have a fully functional asynchronous functionality, by simply adding ``is_async`` tag to the ros2_control tag in the URDF. This will allow the hardware components to run in a separate thread, and the controller manager will be able to run the controllers in parallel with the hardware components.

joint_limits
************
Expand Down
1 change: 1 addition & 0 deletions hardware_interface/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
rclcpp_lifecycle
rcpputils
rcutils
realtime_tools
TinyXML2
tinyxml2_vendor
joint_limits
Expand Down
119 changes: 119 additions & 0 deletions hardware_interface/include/hardware_interface/actuator_interface.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "realtime_tools/async_function_handler.hpp"

namespace hardware_interface
{
Expand Down Expand Up @@ -111,6 +112,30 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
clock_interface_ = clock_interface;
actuator_logger_ = logger.get_child("hardware_component.actuator." + hardware_info.name);
info_ = hardware_info;
if (info_.is_async)
{
RCLCPP_INFO_STREAM(
get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority);
async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
async_handler_->init(
[this](const rclcpp::Time & time, const rclcpp::Duration & period)
{
if (next_trigger_ == TriggerType::READ)
{
const auto ret = read(time, period);
next_trigger_ = TriggerType::WRITE;
return ret;
}
else
{
const auto ret = write(time, period);
next_trigger_ = TriggerType::READ;
return ret;
}
},
info_.thread_priority);
async_handler_->start_thread();
}
return on_init(hardware_info);
};

Expand Down Expand Up @@ -321,6 +346,50 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
return return_type::OK;
}

/// Triggers the read method synchronously or asynchronously depending on the HardwareInfo
/**
* The data readings from the physical hardware has to be updated
* and reflected accordingly in the exported state interfaces.
* That is, the data pointed by the interfaces shall be updated.
* The method is called in the resource_manager's read loop
*
* \param[in] time The time at the start of this control loop iteration
* \param[in] period The measured time taken by the last control loop iteration
* \return return_type::OK if the read was successful, return_type::ERROR otherwise.
*/
return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period)
{
return_type result = return_type::ERROR;
if (info_.is_async)
{
bool trigger_status = true;
if (next_trigger_ == TriggerType::WRITE)
{
RCLCPP_WARN(
get_logger(),
"Trigger read called while write async handler call is still pending for hardware "
"interface : '%s'. Skipping read cycle and will wait for a write cycle!",
info_.name.c_str());
return return_type::OK;
}
std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period);
if (!trigger_status)
{
RCLCPP_WARN(
get_logger(),
"Trigger read called while write async trigger is still in progress for hardware "
"interface : '%s'. Failed to trigger read cycle!",
info_.name.c_str());
return return_type::OK;
}
}
else
{
result = read(time, period);
}
return result;
}

/// Read the current state values from the actuator.
/**
* The data readings from the physical hardware has to be updated
Expand All @@ -333,6 +402,49 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
*/
virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0;

/// Triggers the write method synchronously or asynchronously depending on the HardwareInfo
/**
* The physical hardware shall be updated with the latest value from
* the exported command interfaces.
* The method is called in the resource_manager's write loop
*
* \param[in] time The time at the start of this control loop iteration
* \param[in] period The measured time taken by the last control loop iteration
* \return return_type::OK if the read was successful, return_type::ERROR otherwise.
*/
return_type trigger_write(const rclcpp::Time & time, const rclcpp::Duration & period)
{
return_type result = return_type::ERROR;
if (info_.is_async)
{
bool trigger_status = true;
if (next_trigger_ == TriggerType::READ)
{
RCLCPP_WARN(
get_logger(),
"Trigger write called while read async handler call is still pending for hardware "
"interface : '%s'. Skipping write cycle and will wait for a read cycle!",
info_.name.c_str());
return return_type::OK;
}
std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period);
if (!trigger_status)
{
RCLCPP_WARN(
get_logger(),
"Trigger write called while read async trigger is still in progress for hardware "
"interface : '%s'. Failed to trigger write cycle!",
info_.name.c_str());
return return_type::OK;
}
}
else
{
result = write(time, period);
}
return result;
}

/// Write the current command values to the actuator.
/**
* The physical hardware shall be updated with the latest value from
Expand Down Expand Up @@ -426,13 +538,20 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
std::vector<CommandInterface::SharedPtr> unlisted_commands_;

rclcpp_lifecycle::State lifecycle_state_;
std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;

private:
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
rclcpp::Logger actuator_logger_;
// interface names to Handle accessed through getters/setters
std::unordered_map<std::string, StateInterface::SharedPtr> actuator_states_;
std::unordered_map<std::string, CommandInterface::SharedPtr> actuator_commands_;
enum class TriggerType
{
READ,
WRITE
};
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
std::atomic<TriggerType> next_trigger_ = TriggerType::READ;
};

} // namespace hardware_interface
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -178,6 +178,8 @@ struct HardwareInfo
unsigned int rw_rate;
/// Component is async
bool is_async;
/// Async thread priority
int thread_priority;
/// Name of the pluginlib plugin of the hardware that will be loaded.
std::string hardware_plugin_name;
/// (Optional) Key-value pairs for hardware parameters.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -426,12 +426,6 @@ 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
46 changes: 46 additions & 0 deletions hardware_interface/include/hardware_interface/sensor_interface.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "realtime_tools/async_function_handler.hpp"

namespace hardware_interface
{
Expand Down Expand Up @@ -111,6 +112,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
clock_interface_ = clock_interface;
sensor_logger_ = logger.get_child("hardware_component.sensor." + hardware_info.name);
info_ = hardware_info;
if (info_.is_async)
{
RCLCPP_INFO_STREAM(
get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority);
read_async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
read_async_handler_->init(
std::bind(&SensorInterface::read, this, std::placeholders::_1, std::placeholders::_2),
info_.thread_priority);
read_async_handler_->start_thread();
}
return on_init(hardware_info);
};

Expand Down Expand Up @@ -214,6 +225,40 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
return state_interfaces;
}

/// Triggers the read method synchronously or asynchronously depending on the HardwareInfo
/**
* The data readings from the physical hardware has to be updated
* and reflected accordingly in the exported state interfaces.
* That is, the data pointed by the interfaces shall be updated.
*
* \param[in] time The time at the start of this control loop iteration
* \param[in] period The measured time taken by the last control loop iteration
* \return return_type::OK if the read was successful, return_type::ERROR otherwise.
*/
return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period)
{
return_type result = return_type::ERROR;
if (info_.is_async)
{
bool trigger_status = true;
std::tie(trigger_status, result) = read_async_handler_->trigger_async_callback(time, period);
if (!trigger_status)
{
RCLCPP_WARN(
get_logger(),
"Trigger read called while read async handler is still in progress for hardware "
"interface : '%s'. Failed to trigger read cycle!",
info_.name.c_str());
return return_type::OK;
}
}
else
{
result = read(time, period);
}
return result;
}

/// Read the current state values from the actuator.
/**
* The data readings from the physical hardware has to be updated
Expand Down Expand Up @@ -294,6 +339,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
std::vector<StateInterface::SharedPtr> unlisted_states_;

rclcpp_lifecycle::State lifecycle_state_;
std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> read_async_handler_;

private:
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
Expand Down
Loading
Loading