Skip to content

Commit

Permalink
__ -> _
Browse files Browse the repository at this point in the history
load_urdf() -> load_and_initialize_components()
  • Loading branch information
bmagyar committed Jan 29, 2024
1 parent 19d14ed commit 62a3692
Show file tree
Hide file tree
Showing 5 changed files with 42 additions and 31 deletions.
2 changes: 1 addition & 1 deletion controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -337,7 +337,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &

void ControllerManager::init_resource_manager(const std::string & robot_description)
{
if (!resource_manager_->load_urdf(robot_description))
if (!resource_manager_->load_and_initialize_components(robot_description))
{
RCLCPP_WARN(
get_logger(),
Expand Down
18 changes: 12 additions & 6 deletions controller_manager/test/test_controller_manager_urdf_passing.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,9 +33,13 @@ class TestableControllerManager : public controller_manager::ControllerManager
{
friend TestControllerManagerWithTestableCM;

FRIEND_TEST(TestControllerManagerWithTestableCM, initial_no_load_urdf_called);
FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback);
FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed);
FRIEND_TEST(
TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called);
FRIEND_TEST(
TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback);
FRIEND_TEST(
TestControllerManagerWithTestableCM,
load_and_initialize_components_called_after_invalid_urdf_passed);
FRIEND_TEST(
TestControllerManagerWithTestableCM,
when_starting_broadcaster_expect_error_before_rm_is_initialized_with_robot_description);
Expand Down Expand Up @@ -106,19 +110,21 @@ class TestControllerManagerWithTestableCM
std::shared_ptr<test_controller::TestController> test_controller_;
};

TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called)
TEST_P(TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called)
{
ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
}

TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback)
TEST_P(TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback)
{
ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
pass_robot_description_to_cm_and_rm();
ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());
}

TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed)
TEST_P(
TestControllerManagerWithTestableCM,
load_and_initialize_components_called_after_invalid_urdf_passed)
{
ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
pass_robot_description_to_cm_and_rm(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -90,17 +90,17 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
* initialize the components present in the parsed URDF. Defaults to true.
* \returns false if URDF validation has failed.
*/
bool load_urdf(
bool load_and_initialize_components(
const std::string & urdf, bool validate_interfaces = true,
bool load_and_initialize_components = true);

/**
* @brief if the resource manager load_urdf(...) function has been called this returns true.
* We want to permit to load the urdf later on but we currently don't want to permit multiple
* calls to load_urdf (reloading/loading different urdf).
* @brief if the resource manager load_and_initialize_components(...) function has been called
* this returns true. We want to permit to load the urdf later on but we currently don't want to
* permit multiple calls to load_and_initialize_components (reloading/loading different urdf).
*
* @return true if resource manager's load_urdf() has been already called.
* @return false if resource manager's load_urdf() has not been yet called.
* @return true if resource manager's load_and_initialize_components() has been already called.
* @return false if resource manager's load_and_initialize_components() has not been yet called.
*/
bool is_urdf_already_loaded() const;

Expand Down Expand Up @@ -424,7 +424,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
// Structure to store read and write status so it is not initialized in the real-time loop
HardwareReadWriteStatus read_write_status;

bool is_urdf_loaded__ = false;
bool is_urdf_loaded_ = false;
};

} // namespace hardware_interface
Expand Down
9 changes: 5 additions & 4 deletions hardware_interface/src/resource_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -744,7 +744,7 @@ ResourceManager::ResourceManager(
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
: resource_storage_(std::make_unique<ResourceStorage>(update_rate, clock_interface))
{
load_urdf(urdf, validate_interfaces);
load_and_initialize_components(urdf, validate_interfaces);

if (activate_all)
{
Expand All @@ -758,17 +758,18 @@ ResourceManager::ResourceManager(
}

// CM API: Called in "callback/slow"-thread
bool ResourceManager::load_urdf(
bool ResourceManager::load_and_initialize_components(
const std::string & urdf, bool validate_interfaces, bool load_and_initialize_components)
{
bool result = true;

is_urdf_loaded__ = true;
const std::string system_type = "system";
const std::string sensor_type = "sensor";
const std::string actuator_type = "actuator";

const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
is_urdf_loaded_ = true;

if (load_and_initialize_components)
{
for (const auto & individual_hardware_info : hardware_info)
Expand Down Expand Up @@ -805,7 +806,7 @@ bool ResourceManager::load_urdf(
return result;
}

bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; }
bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded_; }

// CM API: Called in "update"-thread
LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key)
Expand Down
30 changes: 17 additions & 13 deletions hardware_interface_testing/test/test_resource_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -98,23 +98,25 @@ TEST_F(ResourceManagerTest, initialization_with_urdf)
TEST_F(ResourceManagerTest, post_initialization_with_urdf)
{
TestableResourceManager rm;
ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf));
ASSERT_NO_THROW(rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf));
}

TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation)
{
// If the the hardware can not be initialized and load_urdf tried to validate the interfaces a
// runtime exception is thrown
// If the the hardware can not be initialized and load_and_initialize_components tried to validate
// the interfaces a runtime exception is thrown
TestableResourceManager rm;
EXPECT_FALSE(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true));
EXPECT_FALSE(rm.load_and_initialize_components(
ros2_control_test_assets::minimal_unitilizable_robot_urdf, true));
}

TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation)
{
// If the the hardware can not be initialized and load_urdf didn't try to validate the interfaces,
// the interface should not show up
// If the the hardware can not be initialized and load_and_initialize_components didn't try to
// validate the interfaces, the interface should not show up
TestableResourceManager rm;
EXPECT_TRUE(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false));
EXPECT_TRUE(rm.load_and_initialize_components(
ros2_control_test_assets::minimal_unitilizable_robot_urdf, false));

// test actuator
EXPECT_FALSE(rm.state_interface_exists("joint1/position"));
Expand Down Expand Up @@ -167,13 +169,15 @@ TEST_F(ResourceManagerTest, initialization_with_wrong_urdf)
// missing state keys
{
auto rm = TestableResourceManager();
EXPECT_FALSE(rm.load_urdf(ros2_control_test_assets::minimal_robot_missing_state_keys_urdf));
EXPECT_FALSE(rm.load_and_initialize_components(
ros2_control_test_assets::minimal_robot_missing_state_keys_urdf));
ASSERT_TRUE(rm.is_urdf_already_loaded());
}
// missing command keys
{
auto rm = TestableResourceManager();
EXPECT_FALSE(rm.load_urdf(ros2_control_test_assets::minimal_robot_missing_command_keys_urdf));
EXPECT_FALSE(rm.load_and_initialize_components(
ros2_control_test_assets::minimal_robot_missing_command_keys_urdf));
ASSERT_TRUE(rm.is_urdf_already_loaded());
}
}
Expand All @@ -197,23 +201,23 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed)
}
}

TEST_F(ResourceManagerTest, no_load_urdf_function_called)
TEST_F(ResourceManagerTest, no_load_and_initialize_components_function_called)
{
TestableResourceManager rm;
ASSERT_FALSE(rm.is_urdf_already_loaded());
}

TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_valid)
TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_urdf_is_valid)
{
TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf);
ASSERT_TRUE(rm.is_urdf_already_loaded());
}

TEST_F(ResourceManagerTest, can_load_urdf_later)
TEST_F(ResourceManagerTest, can_load_and_initialize_components_later)
{
TestableResourceManager rm;
ASSERT_FALSE(rm.is_urdf_already_loaded());
rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf);
rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf);
ASSERT_TRUE(rm.is_urdf_already_loaded());
}

Expand Down

0 comments on commit 62a3692

Please sign in to comment.