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.

Sign up for GitHub

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

Jump to bottom

[RM] Decouple read/write cycles of each component with mutex to not block other components #1646

Merged
merged 5 commits into from
Aug 14, 2024
Merged

[RM] Decouple read/write cycles of each component with mutex to not block other components #1646

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
f540edb
initial fix for HW components concurrency issue
saikishor Jul 29, 2024
02d08eb
Add the concurrency fix to the System hardware component
saikishor Jul 30, 2024
50fa996
remove the blocking resource locks
saikishor Jul 30, 2024
e10b2f6
readd missing const in get_logger method of actuator and sensor
saikishor Jul 30, 2024
7fbf13e
Merge branch 'master' into fix/hardware_components/concurrency
saikishor Aug 9, 2024
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
4 changes: 3 additions & 1 deletion hardware_interface/include/hardware_interface/actuator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,8 @@ class Actuator final
HARDWARE_INTERFACE_PUBLIC
explicit Actuator(std::unique_ptr<ActuatorInterface> impl);

Actuator(Actuator && other) = default;
HARDWARE_INTERFACE_PUBLIC
explicit Actuator(Actuator && other) noexcept;

~Actuator() = default;

Expand Down Expand Up @@ -102,6 +103,7 @@ class Actuator final

private:
std::unique_ptr<ActuatorInterface> impl_;
mutable std::recursive_mutex actuators_mutex_;
};

} // namespace hardware_interface
Expand Down
2 changes: 1 addition & 1 deletion hardware_interface/include/hardware_interface/actuator_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -228,7 +228,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
*/
void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; }

protected:
/// Get the logger of the ActuatorInterface.
/**
* \return logger of the ActuatorInterface.
Expand All @@ -247,6 +246,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
*/
const HardwareInfo & get_hardware_info() const { return info_; }

protected:
HardwareInfo info_;
rclcpp_lifecycle::State lifecycle_state_;

Expand Down
4 changes: 3 additions & 1 deletion hardware_interface/include/hardware_interface/sensor.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,8 @@ class Sensor final
HARDWARE_INTERFACE_PUBLIC
explicit Sensor(std::unique_ptr<SensorInterface> impl);

Sensor(Sensor && other) = default;
HARDWARE_INTERFACE_PUBLIC
explicit Sensor(Sensor && other) noexcept;

~Sensor() = default;

Expand Down Expand Up @@ -89,6 +90,7 @@ class Sensor final

private:
std::unique_ptr<SensorInterface> impl_;
mutable std::recursive_mutex sensors_mutex_;
};

} // namespace hardware_interface
Expand Down
2 changes: 1 addition & 1 deletion hardware_interface/include/hardware_interface/sensor_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -167,7 +167,6 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
*/
void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; }

protected:
/// Get the logger of the SensorInterface.
/**
* \return logger of the SensorInterface.
Expand All @@ -186,6 +185,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
*/
const HardwareInfo & get_hardware_info() const { return info_; }

protected:
HardwareInfo info_;
rclcpp_lifecycle::State lifecycle_state_;

Expand Down
4 changes: 3 additions & 1 deletion hardware_interface/include/hardware_interface/system.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,8 @@ class System final
HARDWARE_INTERFACE_PUBLIC
explicit System(std::unique_ptr<SystemInterface> impl);

System(System && other) = default;
HARDWARE_INTERFACE_PUBLIC
explicit System(System && other) noexcept;

~System() = default;

Expand Down Expand Up @@ -102,6 +103,7 @@ class System final

private:
std::unique_ptr<SystemInterface> impl_;
mutable std::recursive_mutex system_mutex_;
};

} // namespace hardware_interface
Expand Down
3 changes: 2 additions & 1 deletion hardware_interface/include/hardware_interface/system_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/logger.hpp"
#include "rclcpp/logging.hpp"
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

why don't we need this in the other interface.hpp files?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I've added them in other interfaces.hpp as well

#include "rclcpp/node_interfaces/node_clock_interface.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
Expand Down Expand Up @@ -229,7 +230,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
*/
void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; }

protected:
/// Get the logger of the SystemInterface.
/**
* \return logger of the SystemInterface.
Expand All @@ -248,6 +248,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
*/
const HardwareInfo & get_hardware_info() const { return info_; }

protected:
HardwareInfo info_;
rclcpp_lifecycle::State lifecycle_state_;

Expand Down
30 changes: 30 additions & 0 deletions hardware_interface/src/actuator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"

Expand All @@ -35,10 +36,17 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface

Actuator::Actuator(std::unique_ptr<ActuatorInterface> impl) : impl_(std::move(impl)) {}

Actuator::Actuator(Actuator && other) noexcept
{
std::lock_guard<std::recursive_mutex> lock(other.actuators_mutex_);
impl_ = std::move(other.impl_);
}

const rclcpp_lifecycle::State & Actuator::initialize(
const HardwareInfo & actuator_info, rclcpp::Logger logger,
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
{
std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN)
{
switch (impl_->init(actuator_info, logger, clock_interface))
Expand All @@ -60,6 +68,7 @@ const rclcpp_lifecycle::State & Actuator::initialize(

const rclcpp_lifecycle::State & Actuator::configure()
{
std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
{
switch (impl_->on_configure(impl_->get_state()))
Expand All @@ -83,6 +92,7 @@ const rclcpp_lifecycle::State & Actuator::configure()

const rclcpp_lifecycle::State & Actuator::cleanup()
{
std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
{
switch (impl_->on_cleanup(impl_->get_state()))
Expand All @@ -103,6 +113,7 @@ const rclcpp_lifecycle::State & Actuator::cleanup()

const rclcpp_lifecycle::State & Actuator::shutdown()
{
std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
if (
impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
Expand All @@ -124,6 +135,7 @@ const rclcpp_lifecycle::State & Actuator::shutdown()

const rclcpp_lifecycle::State & Actuator::activate()
{
std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
{
switch (impl_->on_activate(impl_->get_state()))
Expand All @@ -146,6 +158,7 @@ const rclcpp_lifecycle::State & Actuator::activate()

const rclcpp_lifecycle::State & Actuator::deactivate()
{
std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
switch (impl_->on_deactivate(impl_->get_state()))
Expand All @@ -168,6 +181,7 @@ const rclcpp_lifecycle::State & Actuator::deactivate()

const rclcpp_lifecycle::State & Actuator::error()
{
std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN)
{
switch (impl_->on_error(impl_->get_state()))
Expand Down Expand Up @@ -221,6 +235,14 @@ const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_

return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period)
{
std::unique_lock<std::recursive_mutex> lock(actuators_mutex_, std::try_to_lock);
if (!lock.owns_lock())
{
RCLCPP_DEBUG(
impl_->get_logger(), "Skipping read() call for actuator '%s' since it is locked",
impl_->get_name().c_str());
return return_type::OK;
}
if (
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
Expand All @@ -243,6 +265,14 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p

return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period)
{
std::unique_lock<std::recursive_mutex> lock(actuators_mutex_, std::try_to_lock);
if (!lock.owns_lock())
{
RCLCPP_DEBUG(
impl_->get_logger(), "Skipping write() call for actuator '%s' since it is locked",
impl_->get_name().c_str());
return return_type::OK;
}
if (
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
Expand Down
2 changes: 0 additions & 2 deletions hardware_interface/src/resource_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1770,7 +1770,6 @@ void ResourceManager::shutdown_async_components()
HardwareReadWriteStatus ResourceManager::read(
const rclcpp::Time & time, const rclcpp::Duration & period)
{
std::lock_guard<std::recursive_mutex> guard(resources_lock_);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

why don't we need this here? or why do we need this lock in other methods of this file?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The mutex from here is from the Resource Manager, it is used along with non-RT threads, as it is used this way, there is most likely a chance that it can block the read and write cycles.

We have added in other methods because, when this happens, the read cycles of that particular interface can be skipped instead of waiting for the cycle as it is configuring itself to be activated or deactivated

read_write_status.ok = true;
read_write_status.failed_hardware_names.clear();

Expand Down Expand Up @@ -1831,7 +1830,6 @@ HardwareReadWriteStatus ResourceManager::read(
HardwareReadWriteStatus ResourceManager::write(
const rclcpp::Time & time, const rclcpp::Duration & period)
{
std::lock_guard<std::recursive_mutex> guard(resources_lock_);
read_write_status.ok = true;
read_write_status.failed_hardware_names.clear();

Expand Down
22 changes: 22 additions & 0 deletions hardware_interface/src/sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"

Expand All @@ -34,10 +35,17 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface

Sensor::Sensor(std::unique_ptr<SensorInterface> impl) : impl_(std::move(impl)) {}

Sensor::Sensor(Sensor && other) noexcept
{
std::lock_guard<std::recursive_mutex> lock(other.sensors_mutex_);
impl_ = std::move(other.impl_);
}

const rclcpp_lifecycle::State & Sensor::initialize(
const HardwareInfo & sensor_info, rclcpp::Logger logger,
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
{
std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN)
{
switch (impl_->init(sensor_info, logger, clock_interface))
Expand All @@ -59,6 +67,7 @@ const rclcpp_lifecycle::State & Sensor::initialize(

const rclcpp_lifecycle::State & Sensor::configure()
{
std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
{
switch (impl_->on_configure(impl_->get_state()))
Expand All @@ -82,6 +91,7 @@ const rclcpp_lifecycle::State & Sensor::configure()

const rclcpp_lifecycle::State & Sensor::cleanup()
{
std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
{
switch (impl_->on_cleanup(impl_->get_state()))
Expand All @@ -102,6 +112,7 @@ const rclcpp_lifecycle::State & Sensor::cleanup()

const rclcpp_lifecycle::State & Sensor::shutdown()
{
std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
if (
impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
Expand All @@ -123,6 +134,7 @@ const rclcpp_lifecycle::State & Sensor::shutdown()

const rclcpp_lifecycle::State & Sensor::activate()
{
std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
{
switch (impl_->on_activate(impl_->get_state()))
Expand All @@ -145,6 +157,7 @@ const rclcpp_lifecycle::State & Sensor::activate()

const rclcpp_lifecycle::State & Sensor::deactivate()
{
std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
switch (impl_->on_deactivate(impl_->get_state()))
Expand All @@ -167,6 +180,7 @@ const rclcpp_lifecycle::State & Sensor::deactivate()

const rclcpp_lifecycle::State & Sensor::error()
{
std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN)
{
switch (impl_->on_error(impl_->get_state()))
Expand Down Expand Up @@ -199,6 +213,14 @@ const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_st

return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period)
{
std::unique_lock<std::recursive_mutex> lock(sensors_mutex_, std::try_to_lock);
if (!lock.owns_lock())
{
RCLCPP_DEBUG(
impl_->get_logger(), "Skipping read() call for the sensor '%s' since it is locked",
impl_->get_name().c_str());
return return_type::OK;
}
if (
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
Expand Down
Loading
Loading