Skip to content

Commit

Permalink
[RM] Fix crash for missing urdf in resource manager (#1301)
Browse files Browse the repository at this point in the history
* check the components size instead of the variable (fixes #1299)
* add new load_and_initialize_components default argument to the load_urdf method
  • Loading branch information
saikishor authored Jan 17, 2024
1 parent 3309c6d commit 510ba73
Show file tree
Hide file tree
Showing 2 changed files with 24 additions and 16 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -86,8 +86,12 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
* \param[in] urdf string containing the URDF.
* \param[in] validate_interfaces boolean argument indicating whether the exported
* interfaces ought to be validated. Defaults to true.
* \param[in] load_and_initialize_components boolean argument indicating whether to load and
* initialize the components present in the parsed URDF. Defaults to true.
*/
void load_urdf(const std::string & urdf, bool validate_interfaces = true);
void load_urdf(
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.
Expand Down
34 changes: 19 additions & 15 deletions hardware_interface/src/resource_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -756,30 +756,34 @@ ResourceManager::ResourceManager(
}

// CM API: Called in "callback/slow"-thread
void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces)
void ResourceManager::load_urdf(
const std::string & urdf, bool validate_interfaces, bool load_and_initialize_components)
{
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);
for (const auto & individual_hardware_info : hardware_info)
if (load_and_initialize_components)
{
if (individual_hardware_info.type == actuator_type)
for (const auto & individual_hardware_info : hardware_info)
{
std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_);
resource_storage_->load_and_initialize_actuator(individual_hardware_info);
}
if (individual_hardware_info.type == sensor_type)
{
std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
resource_storage_->load_and_initialize_sensor(individual_hardware_info);
}
if (individual_hardware_info.type == system_type)
{
std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_);
resource_storage_->load_and_initialize_system(individual_hardware_info);
if (individual_hardware_info.type == actuator_type)
{
std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_);
resource_storage_->load_and_initialize_actuator(individual_hardware_info);
}
if (individual_hardware_info.type == sensor_type)
{
std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
resource_storage_->load_and_initialize_sensor(individual_hardware_info);
}
if (individual_hardware_info.type == system_type)
{
std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_);
resource_storage_->load_and_initialize_system(individual_hardware_info);
}
}
}

Expand Down

0 comments on commit 510ba73

Please sign in to comment.