Skip to content

Commit

Permalink
use the internal mutex to skip when occupied by other nonRT things
Browse files Browse the repository at this point in the history
  • Loading branch information
@saikishor
saikishor committed Aug 27, 2024
1 parent 3cc4383 commit 881ee78
Show file tree
Hide file tree
Showing 7 changed files with 28 additions and 40 deletions.
3 changes: 3 additions & 0 deletions hardware_interface/include/hardware_interface/actuator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,9 @@ class Actuator final
HARDWARE_INTERFACE_PUBLIC
return_type write(const rclcpp::Time & time, const rclcpp::Duration & period);

HARDWARE_INTERFACE_PUBLIC
std::recursive_mutex & get_mutex();

private:
std::unique_ptr<ActuatorInterface> impl_;
// Last read cycle time
Expand Down
3 changes: 3 additions & 0 deletions hardware_interface/include/hardware_interface/sensor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,9 @@ class Sensor final
HARDWARE_INTERFACE_PUBLIC
return_type write(const rclcpp::Time &, const rclcpp::Duration &) { return return_type::OK; }

HARDWARE_INTERFACE_PUBLIC
std::recursive_mutex & get_mutex();

private:
std::unique_ptr<SensorInterface> impl_;
// Last read cycle time
Expand Down
3 changes: 3 additions & 0 deletions hardware_interface/include/hardware_interface/system.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,9 @@ class System final
HARDWARE_INTERFACE_PUBLIC
return_type write(const rclcpp::Time & time, const rclcpp::Duration & period);

HARDWARE_INTERFACE_PUBLIC
std::recursive_mutex & get_mutex();

private:
std::unique_ptr<SystemInterface> impl_;
// Last read cycle time
Expand Down
17 changes: 1 addition & 16 deletions hardware_interface/src/actuator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -244,14 +244,6 @@ const rclcpp::Time & Actuator::get_last_write_time() const { return last_write_c

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_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
Expand All @@ -276,14 +268,6 @@ 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_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
Expand All @@ -306,4 +290,5 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration &
return result;
}

std::recursive_mutex & Actuator::get_mutex() { return actuators_mutex_; }
} // namespace hardware_interface
16 changes: 16 additions & 0 deletions hardware_interface/src/resource_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1788,6 +1788,14 @@ HardwareReadWriteStatus ResourceManager::read(
{
for (auto & component : components)
{
std::unique_lock<std::recursive_mutex> lock(component.get_mutex(), std::try_to_lock);
if (!lock.owns_lock())
{
RCLCPP_DEBUG(
get_logger(), "Skipping read() call for the component '%s' since it is locked",
component.get_name().c_str());
continue;
}
auto ret_val = return_type::OK;
try
{
Expand Down Expand Up @@ -1864,6 +1872,14 @@ HardwareReadWriteStatus ResourceManager::write(
{
for (auto & component : components)
{
std::unique_lock<std::recursive_mutex> lock(component.get_mutex(), std::try_to_lock);
if (!lock.owns_lock())
{
RCLCPP_DEBUG(
get_logger(), "Skipping write() call for the component '%s' since it is locked",
component.get_name().c_str());
continue;
}
auto ret_val = return_type::OK;
try
{
Expand Down
9 changes: 1 addition & 8 deletions hardware_interface/src/sensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -219,14 +219,6 @@ const rclcpp::Time & Sensor::get_last_read_time() const { return last_read_cycle

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_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
Expand All @@ -249,4 +241,5 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per
return result;
}

std::recursive_mutex & Sensor::get_mutex() { return sensors_mutex_; }
} // namespace hardware_interface
17 changes: 1 addition & 16 deletions hardware_interface/src/system.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -240,14 +240,6 @@ const rclcpp::Time & System::get_last_write_time() const { return last_write_cyc

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

return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period)
{
std::unique_lock<std::recursive_mutex> lock(system_mutex_, std::try_to_lock);
if (!lock.owns_lock())
{
RCLCPP_DEBUG(
impl_->get_logger(), "Skipping write() call for system '%s' since it is locked",
impl_->get_name().c_str());
return return_type::OK;
}
if (
impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
Expand All @@ -302,4 +286,5 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe
return result;
}

std::recursive_mutex & System::get_mutex() { return system_mutex_; }
} // namespace hardware_interface

0 comments on commit 881ee78

Please sign in to comment.