Skip to content

Commit

Permalink
[HW_IF] Prepare the handles for async operations
Browse files Browse the repository at this point in the history
  • Loading branch information
saikishor committed Sep 25, 2024
1 parent 84e85f9 commit fe121c6
Show file tree
Hide file tree
Showing 3 changed files with 106 additions and 8 deletions.
84 changes: 79 additions & 5 deletions hardware_interface/include/hardware_interface/handle.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,9 @@
#define HARDWARE_INTERFACE__HANDLE_HPP_

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

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

Handle(const Handle & other) = default;
Handle(const Handle & other) noexcept
{
std::unique_lock<std::recursive_mutex> lock(other.handle_mutex_);
std::unique_lock<std::recursive_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::recursive_mutex> lock(other.handle_mutex_);
std::unique_lock<std::recursive_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::recursive_mutex> lock(other.handle_mutex_);
std::unique_lock<std::recursive_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::recursive_mutex> lock(other.handle_mutex_);
std::unique_lock<std::recursive_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 @@ -94,21 +140,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::unique_lock<std::recursive_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
{
std::unique_lock<std::recursive_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::recursive_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 @@ -121,6 +194,7 @@ class Handle
// TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed
double * value_ptr_;
// END
mutable std::recursive_mutex handle_mutex_;
};

class StateInterface : public Handle
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

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

#include "hardware_interface/handle.hpp"
Expand Down Expand Up @@ -63,9 +64,23 @@ 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); }
void set_value(double val)
{
while (!command_interface_.set_value(val))
{
std::this_thread::sleep_for(std::chrono::microseconds(10));
}
}

double get_value() const { return command_interface_.get_value(); }
double get_value() const
{
double value;
while (!command_interface_.get_value(value))
{
std::this_thread::sleep_for(std::chrono::microseconds(10));
}
return value;
}

protected:
CommandInterface & command_interface_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

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

#include "hardware_interface/handle.hpp"
Expand Down Expand Up @@ -63,7 +64,15 @@ 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;
while (!state_interface_.get_value(value))
{
std::this_thread::sleep_for(std::chrono::microseconds(10));
}
return value;
}

protected:
StateInterface & state_interface_;
Expand Down

0 comments on commit fe121c6

Please sign in to comment.