Skip to content
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
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ class LoanedCommandInterface
}

explicit LoanedCommandInterface(CommandInterface::SharedPtr command_interface, Deleter && deleter)
: command_interface_(*command_interface), deleter_(std::forward<Deleter>(deleter))
: command_interface_(command_interface), deleter_(std::forward<Deleter>(deleter))
{
}

Expand All @@ -47,38 +47,46 @@ 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 (%.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 * 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 (%.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 * 100.0) / set_value_statistics_.total_counter,
set_value_statistics_.total_counter);
if (command_interface_)
{
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 (%.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 * 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 (%.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 * 100.0) / set_value_statistics_.total_counter,
set_value_statistics_.total_counter);
}
if (deleter_)
{
deleter_();
}
}

const std::string & get_name() const { return command_interface_.get_name(); }
const std::string & get_name() const { return command_interface_->get_name(); }

const std::string & get_interface_name() const { return command_interface_.get_interface_name(); }
const std::string & get_interface_name() const
{
return command_interface_->get_interface_name();
}

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

/**
* @brief Set the value of the command interface.
Expand All @@ -100,7 +108,7 @@ class LoanedCommandInterface
{
unsigned int nr_tries = 0;
++set_value_statistics_.total_counter;
while (!command_interface_.set_limited_value(value))
while (!command_interface_->set_limited_value(value))
{
++set_value_statistics_.failed_counter;
++nr_tries;
Expand Down Expand Up @@ -151,7 +159,7 @@ class LoanedCommandInterface
do
{
++get_value_statistics_.total_counter;
const std::optional<T> data = command_interface_.get_optional<T>();
const std::optional<T> data = command_interface_->get_optional<T>();
if (data.has_value())
{
return data;
Expand All @@ -169,16 +177,16 @@ class LoanedCommandInterface
* @brief Get the data type of the command interface.
* @return The data type of the command interface.
*/
HandleDataType get_data_type() const { return command_interface_.get_data_type(); }
HandleDataType get_data_type() const { return command_interface_->get_data_type(); }

/**
* @brief Check if the state interface can be casted to double.
* @return True if the state interface can be casted to double, false otherwise.
*/
bool is_castable_to_double() const { return command_interface_.is_castable_to_double(); }
bool is_castable_to_double() const { return command_interface_->is_castable_to_double(); }

protected:
CommandInterface & command_interface_;
CommandInterface::SharedPtr command_interface_;
Deleter deleter_;

private:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ class LoanedStateInterface
}

explicit LoanedStateInterface(StateInterface::ConstSharedPtr state_interface, Deleter && deleter)
: state_interface_(*state_interface), deleter_(std::forward<Deleter>(deleter))
: state_interface_(state_interface), deleter_(std::forward<Deleter>(deleter))
{
}

Expand All @@ -46,28 +46,31 @@ 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 * 100.0) / get_value_statistics_.total_counter,
get_value_statistics_.total_counter);
if (state_interface_)
{
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 * 100.0) / get_value_statistics_.total_counter,
get_value_statistics_.total_counter);
}
if (deleter_)
{
deleter_();
}
}

const std::string & get_name() const { return state_interface_.get_name(); }
const std::string & get_name() const { return state_interface_->get_name(); }

const std::string & get_interface_name() const { return state_interface_.get_interface_name(); }
const std::string & get_interface_name() const { return state_interface_->get_interface_name(); }

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

[[deprecated(
"Use std::optional<T> get_optional() instead to retrieve the value. This method will be "
Expand Down Expand Up @@ -106,7 +109,7 @@ class LoanedStateInterface
do
{
++get_value_statistics_.total_counter;
const std::optional<T> data = state_interface_.get_optional<T>();
const std::optional<T> data = state_interface_->get_optional<T>();
if (data.has_value())
{
return data;
Expand All @@ -124,16 +127,16 @@ class LoanedStateInterface
* @brief Get the data type of the state interface.
* @return The data type of the state interface.
*/
HandleDataType get_data_type() const { return state_interface_.get_data_type(); }
HandleDataType get_data_type() const { return state_interface_->get_data_type(); }

/**
* @brief Check if the state interface can be casted to double.
* @return True if the state interface can be casted to double, false otherwise.
*/
bool is_castable_to_double() const { return state_interface_.is_castable_to_double(); }
bool is_castable_to_double() const { return state_interface_->is_castable_to_double(); }

protected:
const StateInterface & state_interface_;
StateInterface::ConstSharedPtr state_interface_;
Deleter deleter_;

private:
Expand Down
Loading