Skip to content

Commit

Permalink
update Loaned command and state handles with statistics, templated se…
Browse files Browse the repository at this point in the history 
…t and get value method with some statistics on misses and timeouts
  • Loading branch information
@saikishor
saikishor committed Oct 14, 2024
1 parent 59c642a commit f0847f3
Show file tree
Hide file tree
Showing 2 changed files with 137 additions and 8 deletions.
89 changes: 84 additions & 5 deletions hardware_interface/include/hardware_interface/loaned_command_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,11 +16,13 @@
#define HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_

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

#include "hardware_interface/handle.hpp"
#include "rclcpp/logging.hpp"

namespace hardware_interface
{
Expand All @@ -33,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 @@ -52,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 @@ -71,27 +100,77 @@ class LoanedCommandInterface

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

void set_value(double val)
template <typename T>
[[nodiscard]] bool set_value(T value, unsigned int max_tries = 10)
{
while (!command_interface_.set_value(val))
unsigned int nr_tries = 0;
set_value_statistics_.total_counter++;
while (!command_interface_.set_value(value))
{
std::this_thread::sleep_for(std::chrono::microseconds(10));
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();
}
}

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))
{
std::this_thread::sleep_for(std::chrono::microseconds(10));
get_value_statistics_.failed_counter++;
++nr_tries;
if (nr_tries == max_tries)
{
get_value_statistics_.timeout_counter++;
return false;
}
std::this_thread::yield();
}
return value;
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
56 changes: 53 additions & 3 deletions hardware_interface/include/hardware_interface/loaned_state_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,12 +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 @@ -57,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 @@ -79,16 +91,54 @@ class LoanedStateInterface
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))
{
std::this_thread::sleep_for(std::chrono::microseconds(10));
get_value_statistics_.failed_counter++;
++nr_tries;
if (nr_tries == max_tries)
{
get_value_statistics_.timeout_counter++;
return false;
}
std::this_thread::yield();
}
return value;
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

0 comments on commit f0847f3

Please sign in to comment.