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

[HW_IF] Prepare the handles for async operations #1750

Merged
merged 7 commits into from
Nov 6, 2024
Merged

[HW_IF] Prepare the handles for async operations #1750

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
59c642a
[HW_IF] Prepare the handles for async operations
saikishor Sep 12, 2024
f0847f3
update Loaned command and state handles with statistics, templated se…
saikishor Oct 14, 2024
f1cd924
Merge branch 'master' into prepare/handles/async
saikishor Oct 14, 2024
466a107
Use shared_mutex for better performance with the StateInterfaces
saikishor Oct 25, 2024
efd8cbf
Merge branch 'master' into prepare/handles/async
saikishor Oct 25, 2024
132c821
Fix the missing reset in LoanedStateInterface and also fix the final …
saikishor Oct 29, 2024
0e3416e
Merge branch 'master' into prepare/handles/async
saikishor Oct 29, 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
85 changes: 80 additions & 5 deletions hardware_interface/include/hardware_interface/handle.hpp
View file
Open in desktop
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
85 changes: 83 additions & 2 deletions hardware_interface/include/hardware_interface/loaned_command_interface.hpp
View file
Open in desktop
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 Down Expand Up @@ -51,6 +54,27 @@ class LoanedCommandInterface

virtual ~LoanedCommandInterface()
{
auto logger = rclcpp::get_logger(command_interface_.get_name());
RCLCPP_WARN_EXPRESSION(
Copy link
Member

Choose a reason for hiding this comment

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

Should we put this to "DEBUG" mode?

Or do we always want output when switching controllers?

Copy link
Member Author

Choose a reason for hiding this comment

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

This is only when we deactivate the controller, i think it's good to have this statistical information.

Moreover, as it is conditioned it should only print for async cases and that too if you have some misses. This should give us some information of the functionality of the system

Copy link
Member Author

Choose a reason for hiding this comment

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

If we have reported issues about this, we can put this to debug in future

rclcpp::get_logger(get_name()),
(get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0),
"LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) 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 * 10.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 (%.4f %%) timeouts and %u (~ %.4f %%) 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 * 10.0) / set_value_statistics_.total_counter,
set_value_statistics_.total_counter);
if (deleter_)
{
deleter_();
Expand All @@ -70,13 +94,70 @@ 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 { return command_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 (!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 = 0;
unsigned int failed_counter = 0;
unsigned int timeout_counter = 0;
};
mutable HandleRTStatistics get_value_statistics_;
HandleRTStatistics set_value_statistics_;
};

} // namespace hardware_interface
Expand Down
56 changes: 54 additions & 2 deletions hardware_interface/include/hardware_interface/loaned_state_interface.hpp
View file
Open in desktop
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 (%.4f %%) timeouts and %u (%.4f %%) 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 * 10.0) / get_value_statistics_.total_counter,
get_value_statistics_.total_counter);
if (deleter_)
{
deleter_();
Expand All @@ -75,11 +88,50 @@ 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 = 0;
unsigned int failed_counter = 0;
unsigned int timeout_counter = 0;
};
mutable HandleRTStatistics get_value_statistics_;
};

} // namespace hardware_interface
Expand Down
Loading