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

[Interfaces] Improved get_name() method of hardware interfaces #api-breaking #737

Merged
merged 15 commits into from
Jul 2, 2022
Merged
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
1 change: 0 additions & 1 deletion .github/workflows/humble-rhel-binary-build.yml
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@ on:
# # Run every day to detect flakiness and broken dependencies
# - cron: '03 1 * * *'


jobs:
humble_rhel_binary:
name: Humble RHEL binary build
Expand Down
7 changes: 4 additions & 3 deletions controller_interface/include/controller_interface/helpers.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,15 +51,16 @@ bool get_ordered_interfaces(
// check case where:
// (<joint> == <joint> AND <interface> == <interface>) OR <joint>/<interface> == 'full name'
if (
((name == interface.get_name()) && (interface_type == interface.get_interface_name())) ||
((name + "/" + interface_type) == interface.get_full_name()))
((name == interface.get_prefix_name()) &&
(interface_type == interface.get_interface_name())) ||
((name + "/" + interface_type) == interface.get_name()))
{
ordered_interfaces.push_back(std::ref(interface));
}
}
else
{
if (name == interface.get_full_name())
if (name == interface.get_name())
{
ordered_interfaces.push_back(std::ref(interface));
}
Expand Down
4 changes: 2 additions & 2 deletions controller_interface/src/chainable_controller_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -67,15 +67,15 @@ ChainableControllerInterface::export_reference_interfaces()
// check if the names of the reference interfaces begin with the controller's name
for (const auto & interface : reference_interfaces)
{
if (interface.get_name() != get_node()->get_name())
if (interface.get_prefix_name() != get_node()->get_name())
{
RCLCPP_FATAL(
get_node()->get_logger(),
"The name of the interface '%s' does not begin with the controller's name. This is "
"mandatory "
" for reference interfaces. No reference interface will be exported. Please correct and "
"recompile the controller with name '%s' and try again.",
interface.get_full_name().c_str(), get_node()->get_name());
interface.get_name().c_str(), get_node()->get_name());
reference_interfaces.clear();
break;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
auto reference_interfaces = controller.export_reference_interfaces();

ASSERT_EQ(reference_interfaces.size(), 1u);
EXPECT_EQ(reference_interfaces[0].get_name(), TEST_CONTROLLER_NAME);
EXPECT_EQ(reference_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME);
EXPECT_EQ(reference_interfaces[0].get_interface_name(), "test_itf");

EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ controller_interface::return_type TestController::update(
{
RCLCPP_INFO(
get_node()->get_logger(), "Setting value of command interface '%s' to %f",
command_interfaces_[i].get_full_name().c_str(), external_commands_for_testing_[i]);
command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]);
command_interfaces_[i].set_value(external_commands_for_testing_[i]);
}

Expand Down
23 changes: 16 additions & 7 deletions hardware_interface/include/hardware_interface/handle.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,9 @@ class ReadOnlyHandle
{
public:
ReadOnlyHandle(
const std::string & name, const std::string & interface_name, double * value_ptr = nullptr)
: name_(name), interface_name_(interface_name), value_ptr_(value_ptr)
const std::string & prefix_name, const std::string & interface_name,
double * value_ptr = nullptr)
: prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr)
{
}

Expand All @@ -56,11 +57,18 @@ class ReadOnlyHandle
/// Returns true if handle references a value.
inline operator bool() const { return value_ptr_ != nullptr; }

const std::string & get_name() const { return name_; }
const std::string get_name() const { return prefix_name_ + "/" + interface_name_; }

const std::string & get_interface_name() const { return interface_name_; }

const std::string get_full_name() const { return name_ + "/" + interface_name_; }
[[deprecated(
"Replaced by get_name method, which is semantically more correct")]] const std::string
get_full_name() const
{
return get_name();
}

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

double get_value() const
{
Expand All @@ -69,7 +77,7 @@ class ReadOnlyHandle
}

protected:
std::string name_;
std::string prefix_name_;
std::string interface_name_;
double * value_ptr_;
};
Expand All @@ -78,8 +86,9 @@ class ReadWriteHandle : public ReadOnlyHandle
{
public:
ReadWriteHandle(
const std::string & name, const std::string & interface_name, double * value_ptr = nullptr)
: ReadOnlyHandle(name, interface_name, value_ptr)
const std::string & prefix_name, const std::string & interface_name,
double * value_ptr = nullptr)
: ReadOnlyHandle(prefix_name, interface_name, value_ptr)
{
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -50,11 +50,18 @@ class LoanedCommandInterface
}
}

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_full_name() const { return command_interface_.get_full_name(); }
[[deprecated(
"Replaced by get_name method, which is semantically more correct")]] const std::string
get_full_name() const
{
return command_interface_.get_name();
}

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

void set_value(double val) { command_interface_.set_value(val); }

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -50,11 +50,18 @@ class LoanedStateInterface
}
}

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_full_name() const { return state_interface_.get_full_name(); }
[[deprecated(
"Replaced by get_name method, which is semantically more correct")]] const std::string
get_full_name() const
{
return state_interface_.get_name();
}

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

double get_value() const { return state_interface_.get_value(); }

Expand Down
4 changes: 2 additions & 2 deletions hardware_interface/src/resource_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -411,7 +411,7 @@ class ResourceStorage
interface_names.reserve(interfaces.size());
for (auto & interface : interfaces)
{
auto key = interface.get_full_name();
auto key = interface.get_name();
state_interface_map_.emplace(std::make_pair(key, std::move(interface)));
interface_names.push_back(key);
}
Expand Down Expand Up @@ -444,7 +444,7 @@ class ResourceStorage
interface_names.reserve(interfaces.size());
for (auto & interface : interfaces)
{
auto key = interface.get_full_name();
auto key = interface.get_name();
command_interface_map_.emplace(std::make_pair(key, std::move(interface)));
claimed_command_interface_map_.emplace(std::make_pair(key, false));
interface_names.push_back(key);
Expand Down
39 changes: 26 additions & 13 deletions hardware_interface/test/test_component_interfaces.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -431,15 +431,18 @@ TEST(TestComponentInterfaces, dummy_actuator)

auto state_interfaces = actuator_hw.export_state_interfaces();
ASSERT_EQ(2u, state_interfaces.size());
EXPECT_EQ("joint1", state_interfaces[0].get_name());
EXPECT_EQ("joint1/position", state_interfaces[0].get_name());
EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0].get_interface_name());
EXPECT_EQ("joint1", state_interfaces[1].get_name());
EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name());
EXPECT_EQ("joint1/velocity", state_interfaces[1].get_name());
EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1].get_interface_name());
EXPECT_EQ("joint1", state_interfaces[1].get_prefix_name());

auto command_interfaces = actuator_hw.export_command_interfaces();
ASSERT_EQ(1u, command_interfaces.size());
EXPECT_EQ("joint1", command_interfaces[0].get_name());
EXPECT_EQ("joint1/velocity", command_interfaces[0].get_name());
EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0].get_interface_name());
EXPECT_EQ("joint1", command_interfaces[0].get_prefix_name());

double velocity_value = 1.0;
command_interfaces[0].set_value(velocity_value); // velocity
Expand Down Expand Up @@ -518,8 +521,9 @@ TEST(TestComponentInterfaces, dummy_sensor)

auto state_interfaces = sensor_hw.export_state_interfaces();
ASSERT_EQ(1u, state_interfaces.size());
EXPECT_EQ("joint1", state_interfaces[0].get_name());
EXPECT_EQ("joint1/voltage", state_interfaces[0].get_name());
EXPECT_EQ("voltage", state_interfaces[0].get_interface_name());
EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name());
EXPECT_TRUE(std::isnan(state_interfaces[0].get_value()));

// Not updated because is is UNCONFIGURED
Expand Down Expand Up @@ -548,27 +552,36 @@ TEST(TestComponentInterfaces, dummy_system)

auto state_interfaces = system_hw.export_state_interfaces();
ASSERT_EQ(6u, state_interfaces.size());
EXPECT_EQ("joint1", state_interfaces[0].get_name());
EXPECT_EQ("joint1/position", state_interfaces[0].get_name());
EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0].get_interface_name());
EXPECT_EQ("joint1", state_interfaces[1].get_name());
EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name());
EXPECT_EQ("joint1/velocity", state_interfaces[1].get_name());
EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1].get_interface_name());
EXPECT_EQ("joint2", state_interfaces[2].get_name());
EXPECT_EQ("joint1", state_interfaces[1].get_prefix_name());
EXPECT_EQ("joint2/position", state_interfaces[2].get_name());
EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[2].get_interface_name());
EXPECT_EQ("joint2", state_interfaces[3].get_name());
EXPECT_EQ("joint2", state_interfaces[2].get_prefix_name());
EXPECT_EQ("joint2/velocity", state_interfaces[3].get_name());
EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[3].get_interface_name());
EXPECT_EQ("joint3", state_interfaces[4].get_name());
EXPECT_EQ("joint2", state_interfaces[3].get_prefix_name());
EXPECT_EQ("joint3/position", state_interfaces[4].get_name());
EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[4].get_interface_name());
EXPECT_EQ("joint3", state_interfaces[5].get_name());
EXPECT_EQ("joint3", state_interfaces[4].get_prefix_name());
EXPECT_EQ("joint3/velocity", state_interfaces[5].get_name());
EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[5].get_interface_name());
EXPECT_EQ("joint3", state_interfaces[5].get_prefix_name());

auto command_interfaces = system_hw.export_command_interfaces();
ASSERT_EQ(3u, command_interfaces.size());
EXPECT_EQ("joint1", command_interfaces[0].get_name());
EXPECT_EQ("joint1/velocity", command_interfaces[0].get_name());
EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0].get_interface_name());
EXPECT_EQ("joint2", command_interfaces[1].get_name());
EXPECT_EQ("joint1", command_interfaces[0].get_prefix_name());
EXPECT_EQ("joint2/velocity", command_interfaces[1].get_name());
EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[1].get_interface_name());
EXPECT_EQ("joint3", command_interfaces[2].get_name());
EXPECT_EQ("joint2", command_interfaces[1].get_prefix_name());
EXPECT_EQ("joint3/velocity", command_interfaces[2].get_name());
EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[2].get_interface_name());
EXPECT_EQ("joint3", command_interfaces[2].get_prefix_name());

double velocity_value = 1.0;
command_interfaces[0].set_value(velocity_value); // velocity
Expand Down
3 changes: 2 additions & 1 deletion hardware_interface/test/test_handle.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,9 @@ TEST(TestHandle, state_interface)
TEST(TestHandle, name_getters_work)
{
StateInterface handle{JOINT_NAME, FOO_INTERFACE};
EXPECT_EQ(handle.get_name(), JOINT_NAME);
EXPECT_EQ(handle.get_name(), std::string(JOINT_NAME) + "/" + std::string(FOO_INTERFACE));
EXPECT_EQ(handle.get_interface_name(), FOO_INTERFACE);
EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME);
}

TEST(TestHandle, value_methods_throw_for_nullptr)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -151,7 +151,7 @@ HandleType get_by_interface(
[&interface_name](const auto handle) { return handle.get_interface_name() == interface_name; });
if (result == handles.cend())
{
return HandleType(handles.cbegin()->get_name(), interface_name, nullptr);
return HandleType(handles.cbegin()->get_prefix_name(), interface_name, nullptr);
}
return *result;
}
Expand All @@ -162,7 +162,7 @@ bool are_names_identical(const std::vector<T> & handles)
std::vector<std::string> names;
std::transform(
handles.cbegin(), handles.cend(), std::back_inserter(names),
[](const auto & handle) { return handle.get_name(); });
[](const auto & handle) { return handle.get_prefix_name(); });
return std::equal(names.cbegin() + 1, names.cend(), names.cbegin());
}

Expand Down