Skip to content

Commit

Permalink
renamed estimated to exported state interface
Browse files Browse the repository at this point in the history
  • Loading branch information
saikishor committed Jul 29, 2023
1 parent 2e380ba commit d2d0760
Show file tree
Hide file tree
Showing 14 changed files with 175 additions and 167 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase
bool is_chainable() const final;

CONTROLLER_INTERFACE_PUBLIC
std::vector<hardware_interface::StateInterface> export_estimated_interfaces() final;
std::vector<hardware_interface::StateInterface> export_state_interfaces() final;

CONTROLLER_INTERFACE_PUBLIC
std::vector<hardware_interface::CommandInterface> export_reference_interfaces() final;
Expand All @@ -75,12 +75,12 @@ class ChainableControllerInterface : public ControllerInterfaceBase
/// chainable interfaces.
/**
* Each chainable controller implements this methods where all its state(read only) interfaces are
* exported. The method has the same meaning as `export_estimated_interfaces` method from
* exported. The method has the same meaning as `export_state_interfaces` method from
* hardware_interface::SystemInterface or hardware_interface::ActuatorInterface.
*
* \returns list of StateInterfaces that other controller can use as their outputs.
*/
virtual std::vector<hardware_interface::StateInterface> on_export_estimated_interfaces() = 0;
virtual std::vector<hardware_interface::StateInterface> on_export_state_interfaces() = 0;

/// Virtual method that each chainable controller should implement to export its read/write
/// chainable interfaces.
Expand Down Expand Up @@ -131,8 +131,8 @@ class ChainableControllerInterface : public ControllerInterfaceBase
virtual return_type update_and_write_commands(
const rclcpp::Time & time, const rclcpp::Duration & period) = 0;

/// Storage of values for estimated interfaces
std::vector<double> estimated_interfaces_data_;
/// Storage of values for exported state interfaces
std::vector<double> exported_state_interfaces_data_;

/// Storage of values for reference interfaces
std::vector<double> reference_interfaces_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase
* \returns empty list.
*/
CONTROLLER_INTERFACE_PUBLIC
std::vector<hardware_interface::StateInterface> export_estimated_interfaces() final;
std::vector<hardware_interface::StateInterface> export_state_interfaces() final;

/**
* Controller has no reference interfaces.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -203,12 +203,12 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
* \returns list of state interfaces for preceding controllers.
*/
CONTROLLER_INTERFACE_PUBLIC
virtual std::vector<hardware_interface::StateInterface> export_estimated_interfaces() = 0;
virtual std::vector<hardware_interface::StateInterface> export_state_interfaces() = 0;

/**
* Set chained mode of a chainable controller. This method triggers internal processes to switch
* a chainable controller to "chained" mode and vice-versa. Setting controller to "chained" mode
* usually involves the usage of the controller's reference/estimated interfaces by the other
* usually involves the usage of the controller's reference/state interfaces by the other
* controllers
*
* \returns true if mode is switched successfully and false if not.
Expand Down
26 changes: 13 additions & 13 deletions controller_interface/src/chainable_controller_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -45,41 +45,41 @@ return_type ChainableControllerInterface::update(
}

std::vector<hardware_interface::StateInterface>
ChainableControllerInterface::export_estimated_interfaces()
ChainableControllerInterface::export_state_interfaces()
{
auto estimated_interfaces = on_export_estimated_interfaces();
// check if the "estimated_interfaces_data_" variable is resized to number of interfaces
if (estimated_interfaces_data_.size() != estimated_interfaces.size())
auto state_interfaces = on_export_state_interfaces();
// check if the "state_interfaces_data_" variable is resized to number of interfaces
if (exported_state_interfaces_data_.size() != state_interfaces.size())
{
// TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the
// framework
RCLCPP_FATAL(
get_node()->get_logger(),
"The internal storage for estimated values 'estimated_interfaces_data_' variable has size "
"'%zu', but it is expected to have the size '%zu' equal to the number of exported reference "
"interfaces. No reference interface will be exported. Please correct and recompile "
"The internal storage for exported state values 'state_interfaces_data_' variable has size "
"'%zu', but it is expected to have the size '%zu' equal to the number of exported state "
"interfaces. No state interface will be exported. Please correct and recompile "
"the controller with name '%s' and try again.",
estimated_interfaces_data_.size(), estimated_interfaces.size(), get_node()->get_name());
estimated_interfaces.clear();
exported_state_interfaces_data_.size(), state_interfaces.size(), get_node()->get_name());
state_interfaces.clear();
}

// check if the names of the controller state interfaces begin with the controller's name
for (const auto & interface : estimated_interfaces)
for (const auto & interface : state_interfaces)
{
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 estimated interfaces. No estimated interface will be exported. Please "
"mandatory for state interfaces. No state interface will be exported. Please "
"correct and recompile the controller with name '%s' and try again.",
interface.get_name().c_str(), get_node()->get_name());
estimated_interfaces.clear();
state_interfaces.clear();
break;
}
}

return estimated_interfaces;
return state_interfaces;
}

std::vector<hardware_interface::CommandInterface>
Expand Down
2 changes: 1 addition & 1 deletion controller_interface/src/controller_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {}

bool ControllerInterface::is_chainable() const { return false; }

std::vector<hardware_interface::StateInterface> ControllerInterface::export_estimated_interfaces()
std::vector<hardware_interface::StateInterface> ControllerInterface::export_state_interfaces()
{
return {};
}
Expand Down
42 changes: 22 additions & 20 deletions controller_interface/test/test_chainable_controller_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,21 +28,21 @@ TEST_F(ChainableControllerInterfaceTest, default_returns)
EXPECT_FALSE(controller.is_in_chained_mode());
}

TEST_F(ChainableControllerInterfaceTest, export_estimated_interfaces)
TEST_F(ChainableControllerInterfaceTest, export_state_interfaces)
{
TestableChainableControllerInterface controller;

// initialize, create node
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
ASSERT_NO_THROW(controller.get_node());

auto estimated_interfaces = controller.export_estimated_interfaces();
auto exported_state_interfaces = controller.export_state_interfaces();

ASSERT_EQ(estimated_interfaces.size(), 1u);
EXPECT_EQ(estimated_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME);
EXPECT_EQ(estimated_interfaces[0].get_interface_name(), "test_state");
ASSERT_EQ(exported_state_interfaces.size(), 1u);
EXPECT_EQ(exported_state_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME);
EXPECT_EQ(exported_state_interfaces[0].get_interface_name(), "test_state");

EXPECT_EQ(estimated_interfaces[0].get_value(), ESTIMATED_INTERFACE_VALUE);
EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE);
}

TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
Expand Down Expand Up @@ -75,9 +75,9 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_storage_not_correct_size)
auto reference_interfaces = controller.export_reference_interfaces();
ASSERT_TRUE(reference_interfaces.empty());
// expect empty return because storage is not resized
controller.estimated_interfaces_data_.clear();
auto estimated_interfaces = controller.export_estimated_interfaces();
ASSERT_TRUE(estimated_interfaces.empty());
controller.exported_state_interfaces_data_.clear();
auto exported_state_interfaces = controller.export_state_interfaces();
ASSERT_TRUE(exported_state_interfaces.empty());
}

TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name)
Expand All @@ -94,8 +94,8 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name)
auto reference_interfaces = controller.export_reference_interfaces();
ASSERT_TRUE(reference_interfaces.empty());
// expect empty return because interface prefix is not equal to the node name
auto estimated_interfaces = controller.export_estimated_interfaces();
ASSERT_TRUE(estimated_interfaces.empty());
auto exported_state_interfaces = controller.export_state_interfaces();
ASSERT_TRUE(exported_state_interfaces.empty());
}

TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
Expand All @@ -108,14 +108,14 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)

auto reference_interfaces = controller.export_reference_interfaces();
ASSERT_EQ(reference_interfaces.size(), 1u);
auto estimated_interfaces = controller.export_estimated_interfaces();
ASSERT_EQ(estimated_interfaces.size(), 1u);
auto exported_state_interfaces = controller.export_state_interfaces();
ASSERT_EQ(exported_state_interfaces.size(), 1u);

EXPECT_FALSE(controller.is_in_chained_mode());

// Fail setting chained mode
EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE);
EXPECT_EQ(estimated_interfaces[0].get_value(), ESTIMATED_INTERFACE_VALUE);
EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE);

EXPECT_FALSE(controller.set_chained_mode(true));
EXPECT_FALSE(controller.is_in_chained_mode());
Expand All @@ -128,7 +128,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)

EXPECT_TRUE(controller.set_chained_mode(true));
EXPECT_TRUE(controller.is_in_chained_mode());
EXPECT_EQ(estimated_interfaces[0].get_value(), ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE);
EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE);

controller.configure();
EXPECT_TRUE(controller.set_chained_mode(false));
Expand Down Expand Up @@ -165,23 +165,23 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic)
controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);
ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1);
ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE + 1);
ASSERT_EQ(controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE + 1);

// Provoke error in update from subscribers - return ERROR and update_and_write_commands not exec.
controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR);
ASSERT_EQ(
controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::ERROR);
ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1);
ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE + 1);
ASSERT_EQ(controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE + 1);

// Provoke error from update - return ERROR, but reference interface is updated and not reduced
controller.set_new_reference_interface_value(INTERFACE_VALUE_UPDATE_ERROR);
ASSERT_EQ(
controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::ERROR);
ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR);
ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE + 1);
ASSERT_EQ(controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE + 1);

controller.reference_interfaces_[0] = 0.0;

Expand All @@ -196,7 +196,8 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic)
controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);
ASSERT_EQ(controller.reference_interfaces_[0], -1.0);
ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE + 1);
ASSERT_EQ(
controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1);

// Provoke error from update - return ERROR, but reference interface is updated directly
controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR);
Expand All @@ -205,5 +206,6 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic)
controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::ERROR);
ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR);
ASSERT_EQ(controller.estimated_interfaces_data_[0], ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE + 1);
ASSERT_EQ(
controller.exported_state_interfaces_data_[0], EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1);
}
22 changes: 11 additions & 11 deletions controller_interface/test/test_chainable_controller_interface.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,8 @@ constexpr double INTERFACE_VALUE = 1989.0;
constexpr double INTERFACE_VALUE_SUBSCRIBER_ERROR = 12345.0;
constexpr double INTERFACE_VALUE_UPDATE_ERROR = 67890.0;
constexpr double INTERFACE_VALUE_INITIAL_REF = 1984.0;
constexpr double ESTIMATED_INTERFACE_VALUE = 21833.0;
constexpr double ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE = 82802.0;
constexpr double EXPORTED_STATE_INTERFACE_VALUE = 21833.0;
constexpr double EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE = 82802.0;

class TestableChainableControllerInterface
: public controller_interface::ChainableControllerInterface
Expand All @@ -43,8 +43,8 @@ class TestableChainableControllerInterface
{
reference_interfaces_.reserve(1);
reference_interfaces_.push_back(INTERFACE_VALUE);
estimated_interfaces_data_.reserve(1);
estimated_interfaces_data_.push_back(ESTIMATED_INTERFACE_VALUE);
exported_state_interfaces_data_.reserve(1);
exported_state_interfaces_data_.push_back(EXPORTED_STATE_INTERFACE_VALUE);
}

controller_interface::CallbackReturn on_init() override
Expand All @@ -68,14 +68,14 @@ class TestableChainableControllerInterface
}

// Implementation of ChainableController virtual methods
std::vector<hardware_interface::StateInterface> on_export_estimated_interfaces() override
std::vector<hardware_interface::StateInterface> on_export_state_interfaces() override
{
std::vector<hardware_interface::StateInterface> estimated_interfaces;
std::vector<hardware_interface::StateInterface> state_interfaces;

estimated_interfaces.push_back(hardware_interface::StateInterface(
name_prefix_of_interfaces_, "test_state", &estimated_interfaces_data_[0]));
state_interfaces.push_back(hardware_interface::StateInterface(
name_prefix_of_interfaces_, "test_state", &exported_state_interfaces_data_[0]));

return estimated_interfaces;
return state_interfaces;
}

// Implementation of ChainableController virtual methods
Expand All @@ -93,7 +93,7 @@ class TestableChainableControllerInterface
{
if (reference_interfaces_[0] == 0.0)
{
estimated_interfaces_data_[0] = ESTIMATED_INTERFACE_VALUE_IN_CHAINMODE;
exported_state_interfaces_data_[0] = EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE;
return true;
}
else
Expand Down Expand Up @@ -123,7 +123,7 @@ class TestableChainableControllerInterface
}

reference_interfaces_[0] -= 1;
estimated_interfaces_data_[0] += 1;
exported_state_interfaces_data_[0] += 1;

return controller_interface::return_type::OK;
}
Expand Down
Loading

0 comments on commit d2d0760

Please sign in to comment.