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

[HW_IF] Prepare the handles for async operations #1750

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open
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
85 changes: 80 additions & 5 deletions hardware_interface/include/hardware_interface/handle.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,10 @@

#include <limits>
#include <memory>
#include <mutex>
#include <shared_mutex>
#include <string>
#include <utility>
#include <variant>

#include "hardware_interface/hardware_info.hpp"
Expand Down Expand Up @@ -69,13 +72,57 @@ class Handle
{
}

Handle(const Handle & other) = default;
Handle(const Handle & other) noexcept
{
std::unique_lock<std::shared_mutex> lock(other.handle_mutex_);
std::unique_lock<std::shared_mutex> lock_this(handle_mutex_);
prefix_name_ = other.prefix_name_;
interface_name_ = other.interface_name_;
handle_name_ = other.handle_name_;
value_ = other.value_;
value_ptr_ = other.value_ptr_;
}

Handle(Handle && other) = default;
Handle(Handle && other) noexcept
{
std::unique_lock<std::shared_mutex> lock(other.handle_mutex_);
std::unique_lock<std::shared_mutex> lock_this(handle_mutex_);
prefix_name_ = std::move(other.prefix_name_);
interface_name_ = std::move(other.interface_name_);
handle_name_ = std::move(other.handle_name_);
value_ = std::move(other.value_);
value_ptr_ = std::move(other.value_ptr_);
}

Handle & operator=(const Handle & other) = default;
Handle & operator=(const Handle & other)
{
if (this != &other)
{
std::unique_lock<std::shared_mutex> lock(other.handle_mutex_);
std::unique_lock<std::shared_mutex> lock_this(handle_mutex_);
prefix_name_ = other.prefix_name_;
interface_name_ = other.interface_name_;
handle_name_ = other.handle_name_;
value_ = other.value_;
value_ptr_ = other.value_ptr_;
}
return *this;
}

Handle & operator=(Handle && other) = default;
Handle & operator=(Handle && other)
{
if (this != &other)
{
std::unique_lock<std::shared_mutex> lock(other.handle_mutex_);
std::unique_lock<std::shared_mutex> lock_this(handle_mutex_);
prefix_name_ = std::move(other.prefix_name_);
interface_name_ = std::move(other.interface_name_);
handle_name_ = std::move(other.handle_name_);
value_ = std::move(other.value_);
value_ptr_ = std::move(other.value_ptr_);
}
return *this;
}

virtual ~Handle() = default;

Expand All @@ -95,21 +142,48 @@ class Handle

const std::string & get_prefix_name() const { return prefix_name_; }

[[deprecated("Use bool get_value(double & value) instead to retrieve the value.")]]
double get_value() const
{
std::shared_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
if (!lock.owns_lock())
{
return std::numeric_limits<double>::quiet_NaN();
}
// BEGIN (Handle export change): for backward compatibility
// TODO(Manuel) return value_ if old functionality is removed
THROW_ON_NULLPTR(value_ptr_);
return *value_ptr_;
// END
}

void set_value(double value)
[[nodiscard]] bool get_value(double & value) const
Copy link
Member

Choose a reason for hiding this comment

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

@mamueluth please check this. This is how we can template the access to variables. This should simplify test changes.

{
std::shared_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
if (!lock.owns_lock())
{
return false;
}
// BEGIN (Handle export change): for backward compatibility
// TODO(Manuel) set value directly if old functionality is removed
THROW_ON_NULLPTR(value_ptr_);
value = *value_ptr_;
return true;
// END
}

[[nodiscard]] bool set_value(double value)
{
std::unique_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
if (!lock.owns_lock())
{
return false;
}
// BEGIN (Handle export change): for backward compatibility
// TODO(Manuel) set value_ directly if old functionality is removed
THROW_ON_NULLPTR(this->value_ptr_);
*this->value_ptr_ = value;
return true;
// END
}

Expand All @@ -122,6 +196,7 @@ class Handle
// TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed
double * value_ptr_;
// END
mutable std::shared_mutex handle_mutex_;
};

class StateInterface : public Handle
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,10 +16,13 @@
#define HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_

#include <functional>
#include <limits>
#include <string>
#include <thread>
#include <utility>

#include "hardware_interface/handle.hpp"
saikishor marked this conversation as resolved.
Show resolved Hide resolved
#include "rclcpp/logging.hpp"

namespace hardware_interface
{
Expand All @@ -32,17 +35,23 @@ class LoanedCommandInterface
CommandInterface & command_interface)
: LoanedCommandInterface(command_interface, nullptr)
{
get_value_statistics_.reset();
set_value_statistics_.reset();
}

[[deprecated("Replaced by the new version using shared_ptr")]] LoanedCommandInterface(
CommandInterface & command_interface, Deleter && deleter)
: command_interface_(command_interface), deleter_(std::forward<Deleter>(deleter))
{
get_value_statistics_.reset();
set_value_statistics_.reset();
}

LoanedCommandInterface(CommandInterface::SharedPtr command_interface, Deleter && deleter)
: command_interface_(*command_interface), deleter_(std::forward<Deleter>(deleter))
{
get_value_statistics_.reset();
set_value_statistics_.reset();
}

LoanedCommandInterface(const LoanedCommandInterface & other) = delete;
Expand All @@ -51,6 +60,27 @@ class LoanedCommandInterface

virtual ~LoanedCommandInterface()
{
auto logger = rclcpp::get_logger(command_interface_.get_name());
RCLCPP_WARN_EXPRESSION(
rclcpp::get_logger(get_name()),
(get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0),
"LoanedCommandInterface %s has %u (%f %%) timeouts and %u (%f %%) missed calls out of %u "
"get_value calls",
get_name().c_str(), get_value_statistics_.timeout_counter,
(get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter,
get_value_statistics_.failed_counter,
(get_value_statistics_.failed_counter * 100.0) / get_value_statistics_.total_counter,
get_value_statistics_.total_counter);
RCLCPP_WARN_EXPRESSION(
rclcpp::get_logger(get_name()),
(set_value_statistics_.failed_counter > 0 || set_value_statistics_.timeout_counter > 0),
"LoanedCommandInterface %s has %u (%f %%) timeouts and %u (%f %%) missed calls out of %u "
"set_value calls",
get_name().c_str(), set_value_statistics_.timeout_counter,
(set_value_statistics_.timeout_counter * 100.0) / set_value_statistics_.total_counter,
set_value_statistics_.failed_counter,
(set_value_statistics_.failed_counter * 100.0) / set_value_statistics_.total_counter,
set_value_statistics_.total_counter);
if (deleter_)
{
deleter_();
Expand All @@ -70,13 +100,77 @@ class LoanedCommandInterface

const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); }

void set_value(double val) { command_interface_.set_value(val); }
template <typename T>
[[nodiscard]] bool set_value(T value, unsigned int max_tries = 10)
{
unsigned int nr_tries = 0;
set_value_statistics_.total_counter++;
while (!command_interface_.set_value(value))
{
set_value_statistics_.failed_counter++;
++nr_tries;
if (nr_tries == max_tries)
{
set_value_statistics_.timeout_counter++;
return false;
}
std::this_thread::yield();
}
return true;
}

double get_value() const
{
double value;
if (get_value(value))
{
return value;
}
else
{
return std::numeric_limits<double>::quiet_NaN();
}
}

double get_value() const { return command_interface_.get_value(); }
template <typename T>
[[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const
{
unsigned int nr_tries = 0;
get_value_statistics_.total_counter++;
while (!command_interface_.get_value(value))
{
get_value_statistics_.failed_counter++;
++nr_tries;
if (nr_tries == max_tries)
{
get_value_statistics_.timeout_counter++;
return false;
}
std::this_thread::yield();
}
return true;
}

protected:
CommandInterface & command_interface_;
Deleter deleter_;

private:
struct HandleRTStatistics
{
unsigned int total_counter;
unsigned int failed_counter;
unsigned int timeout_counter;

void reset()
{
total_counter = 0;
failed_counter = 0;
timeout_counter = 0;
}
};
mutable HandleRTStatistics get_value_statistics_;
HandleRTStatistics set_value_statistics_;
};

} // namespace hardware_interface
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,11 +16,13 @@
#define HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_

#include <functional>
#include <limits>
#include <string>
#include <thread>
#include <utility>

#include "hardware_interface/handle.hpp"

#include "rclcpp/logging.hpp"
namespace hardware_interface
{
class LoanedStateInterface
Expand Down Expand Up @@ -56,6 +58,17 @@ class LoanedStateInterface

virtual ~LoanedStateInterface()
{
auto logger = rclcpp::get_logger(state_interface_.get_name());
RCLCPP_WARN_EXPRESSION(
logger,
(get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0),
"LoanedStateInterface %s has %u (%.2f %%) timeouts and %u (%.2f %%) missed calls out of %u "
"get_value calls",
state_interface_.get_name().c_str(), get_value_statistics_.timeout_counter,
(get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter,
get_value_statistics_.failed_counter,
(get_value_statistics_.failed_counter * 100.0) / get_value_statistics_.total_counter,
get_value_statistics_.total_counter);
if (deleter_)
{
deleter_();
Expand All @@ -75,11 +88,57 @@ class LoanedStateInterface

const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); }

double get_value() const { return state_interface_.get_value(); }
double get_value() const
{
double value;
if (get_value(value))
{
return value;
}
else
{
return std::numeric_limits<double>::quiet_NaN();
}
}

template <typename T>
[[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const
{
unsigned int nr_tries = 0;
get_value_statistics_.total_counter++;
while (!state_interface_.get_value(value))
{
get_value_statistics_.failed_counter++;
++nr_tries;
if (nr_tries == max_tries)
{
get_value_statistics_.timeout_counter++;
return false;
}
std::this_thread::yield();
}
return true;
}

protected:
const StateInterface & state_interface_;
Deleter deleter_;

private:
struct HandleRTStatistics
{
unsigned int total_counter;
unsigned int failed_counter;
unsigned int timeout_counter;

void reset()
{
total_counter = 0;
failed_counter = 0;
timeout_counter = 0;
}
};
mutable HandleRTStatistics get_value_statistics_;
};

} // namespace hardware_interface
Expand Down
Loading