Skip to content

Commit

Permalink
[MockHardware] Enalbe initialization non-joint components(#822)
Browse files Browse the repository at this point in the history 
Co-authored-by: Denis Štogl <denis@stoglrobotics.de>
  • Loading branch information
@fmauch @destogl
fmauch and destogl committed Oct 21, 2022
1 parent 8559be7 commit f1302cb
Show file tree
Hide file tree
Showing 3 changed files with 100 additions and 14 deletions.
3 changes: 2 additions & 1 deletion hardware_interface/include/mock_components/generic_system.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,8 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste

void initialize_storage_vectors(
std::vector<std::vector<double>> & commands, std::vector<std::vector<double>> & states,
const std::vector<std::string> & interfaces);
const std::vector<std::string> & interfaces,
const std::vector<hardware_interface::ComponentInfo> & component_infos);

template <typename InterfaceType>
bool populate_interfaces(
Expand Down
28 changes: 15 additions & 13 deletions hardware_interface/src/mock_components/generic_system.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,7 +112,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i
index_custom_interface_with_following_offset_ = std::numeric_limits<size_t>::max();

// Initialize storage for standard interfaces
initialize_storage_vectors(joint_commands_, joint_states_, standard_interfaces_);
initialize_storage_vectors(joint_commands_, joint_states_, standard_interfaces_, info_.joints);
// set all values without initial values to 0
for (auto i = 0u; i < info_.joints.size(); i++)
{
Expand Down Expand Up @@ -164,7 +164,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i
}

// Initialize storage for non-standard interfaces
initialize_storage_vectors(other_commands_, other_states_, other_interfaces_);
initialize_storage_vectors(other_commands_, other_states_, other_interfaces_, info_.joints);

// when following offset is used on custom interface then find its index
if (!custom_interface_with_following_offset_.empty())
Expand Down Expand Up @@ -201,7 +201,8 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i
}
}
}
initialize_storage_vectors(sensor_fake_commands_, sensor_states_, sensor_interfaces_);
initialize_storage_vectors(
sensor_fake_commands_, sensor_states_, sensor_interfaces_, info_.sensors);

// search for gpio interfaces
for (const auto & gpio : info_.gpios)
Expand All @@ -216,12 +217,12 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i
// Fake gpio command interfaces
if (use_fake_gpio_command_interfaces_)
{
initialize_storage_vectors(gpio_fake_commands_, gpio_states_, gpio_interfaces_);
initialize_storage_vectors(gpio_fake_commands_, gpio_states_, gpio_interfaces_, info_.gpios);
}
// Real gpio command interfaces
else
{
initialize_storage_vectors(gpio_commands_, gpio_states_, gpio_interfaces_);
initialize_storage_vectors(gpio_commands_, gpio_states_, gpio_interfaces_, info_.gpios);
}

return CallbackReturn::SUCCESS;
Expand Down Expand Up @@ -425,23 +426,24 @@ bool GenericSystem::get_interface(

void GenericSystem::initialize_storage_vectors(
std::vector<std::vector<double>> & commands, std::vector<std::vector<double>> & states,
const std::vector<std::string> & interfaces)
const std::vector<std::string> & interfaces,
const std::vector<hardware_interface::ComponentInfo> & component_infos)
{
// Initialize storage for all joints, regardless of their existence
commands.resize(interfaces.size());
states.resize(interfaces.size());
for (auto i = 0u; i < interfaces.size(); i++)
{
commands[i].resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
states[i].resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
commands[i].resize(component_infos.size(), std::numeric_limits<double>::quiet_NaN());
states[i].resize(component_infos.size(), std::numeric_limits<double>::quiet_NaN());
}

// Initialize with values from URDF
bool print_hint = false;
for (auto i = 0u; i < info_.joints.size(); i++)
for (auto i = 0u; i < component_infos.size(); i++)
{
const auto & joint = info_.joints[i];
for (const auto & interface : joint.state_interfaces)
const auto & component = component_infos[i];
for (const auto & interface : component.state_interfaces)
{
auto it = std::find(interfaces.begin(), interfaces.end(), interface.name);

Expand All @@ -458,8 +460,8 @@ void GenericSystem::initialize_storage_vectors(
else
{
// Initialize the value in old way with warning message
auto it2 = joint.parameters.find("initial_" + interface.name);
if (it2 != joint.parameters.end())
auto it2 = component.parameters.find("initial_" + interface.name);
if (it2 != component.parameters.end())
{
states[index][i] = std::stod(it2->second);
print_hint = true;
Expand Down
83 changes: 83 additions & 0 deletions hardware_interface/test/mock_components/test_generic_system.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -414,6 +414,40 @@ class TestGenericSystem : public ::testing::Test
</gpio>
</ros2_control>
)";

sensor_with_initial_value_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>fake_components/GenericSystem</plugin>
</hardware>
<sensor name="force_sensor">
<state_interface name="force.x">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="force.y">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="force.z">
<param name="initial_value">0.0</param>
</state_interface>
</sensor>
</ros2_control>
)";

gpio_with_initial_value_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>fake_components/GenericSystem</plugin>
</hardware>
<gpio name="sample_io">
<state_interface name="output_1">
<param name="initial_value">1</param>
</state_interface>
</gpio>
</ros2_control>
)";
}

std::string hardware_robot_2dof_;
Expand All @@ -431,6 +465,8 @@ class TestGenericSystem : public ::testing::Test
std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_;
std::string valid_urdf_ros2_control_system_robot_with_gpio_;
std::string valid_urdf_ros2_control_system_robot_with_gpio_fake_command_;
std::string sensor_with_initial_value_;
std::string gpio_with_initial_value_;
};

// Forward declaration
Expand Down Expand Up @@ -1470,3 +1506,50 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_co
ASSERT_EQ(1.11, gpio1_a_i2_c.get_value());
ASSERT_EQ(2.22, gpio2_vac_c.get_value());
}

TEST_F(TestGenericSystem, sensor_with_initial_value_)
{
auto urdf = ros2_control_test_assets::urdf_head + sensor_with_initial_value_ +
ros2_control_test_assets::urdf_tail;
TestableResourceManager rm(urdf);
// Activate components to get all interfaces available
activate_components(rm);

// Check interfaces
EXPECT_EQ(1u, rm.system_components_size());
ASSERT_EQ(3u, rm.state_interface_keys().size());
EXPECT_TRUE(rm.state_interface_exists("force_sensor/force.x"));
EXPECT_TRUE(rm.state_interface_exists("force_sensor/force.y"));
EXPECT_TRUE(rm.state_interface_exists("force_sensor/force.z"));

// Check initial values
hardware_interface::LoanedStateInterface force_x_s =
rm.claim_state_interface("force_sensor/force.x");
hardware_interface::LoanedStateInterface force_y_s =
rm.claim_state_interface("force_sensor/force.y");
hardware_interface::LoanedStateInterface force_z_s =
rm.claim_state_interface("force_sensor/force.z");

ASSERT_EQ(0.0, force_x_s.get_value());
ASSERT_EQ(0.0, force_y_s.get_value());
ASSERT_EQ(0.0, force_z_s.get_value());
}

TEST_F(TestGenericSystem, gpio_with_initial_value_)
{
auto urdf = ros2_control_test_assets::urdf_head + gpio_with_initial_value_ +
ros2_control_test_assets::urdf_tail;
TestableResourceManager rm(urdf);
// Activate components to get all interfaces available
activate_components(rm);

// Check interfaces
EXPECT_EQ(1u, rm.system_components_size());
ASSERT_EQ(1u, rm.state_interface_keys().size());
EXPECT_TRUE(rm.state_interface_exists("sample_io/output_1"));

// Check initial values
hardware_interface::LoanedStateInterface state = rm.claim_state_interface("sample_io/output_1");

ASSERT_EQ(1, state.get_value());
}

0 comments on commit f1302cb

Please sign in to comment.