Skip to content

Commit

Permalink
Propagate read/write rate to the HardwareInfo properly
Browse files Browse the repository at this point in the history
saikishor committed Dec 10, 2024
1 parent d40377d commit a386668
Showing 4 changed files with 19 additions and 13 deletions.
Original file line number Diff line number Diff line change
@@ -27,11 +27,13 @@ namespace hardware_interface
/// Search XML snippet from URDF for information about a control component.
/**
* \param[in] urdf string with robot's URDF
* \param[in] cm_update_rate The update rate of the controller manager
* \return vector filled with information about robot's control resources
* \throws std::runtime_error if a robot attribute or tag is not found
*/
HARDWARE_INTERFACE_PUBLIC
std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string & urdf);
std::vector<HardwareInfo> parse_control_resources_from_urdf(
const std::string & urdf, const unsigned int cm_update_rate = 100);

/**
* \param[in] component_info information about a component (gpio, joint, sensor)
16 changes: 10 additions & 6 deletions hardware_interface/src/component_parser.cpp
Original file line number Diff line number Diff line change
@@ -231,12 +231,13 @@ std::string parse_data_type_attribute(const tinyxml2::XMLElement * elem)
* \param[in] elem XMLElement that has the rw_rate attribute.
* \return unsigned int specifying the read/write rate.
*/
unsigned int parse_rw_rate_attribute(const tinyxml2::XMLElement * elem)
unsigned int parse_rw_rate_attribute(
const tinyxml2::XMLElement * elem, const unsigned int cm_update_rate)
{
const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kReadWriteRateAttribute);
try
{
const auto rw_rate = attr ? std::stoi(attr->Value()) : 0;
const int rw_rate = attr ? std::stoi(attr->Value()) : static_cast<int>(cm_update_rate);
if (rw_rate < 0)
{
throw std::runtime_error(
@@ -603,16 +604,18 @@ void auto_fill_transmission_interfaces(HardwareInfo & hardware)
/**
* \param[in] ros2_control_it pointer to ros2_control element
* with information about resource.
* \param[in] cm_update_rate The update rate of the controller manager
* \return HardwareInfo filled with information about the robot
* \throws std::runtime_error if a attributes or tag are not found
*/
HardwareInfo parse_resource_from_xml(
const tinyxml2::XMLElement * ros2_control_it, const std::string & urdf)
const tinyxml2::XMLElement * ros2_control_it, const std::string & urdf,
const unsigned int cm_update_rate)
{
HardwareInfo hardware;
hardware.name = get_attribute_value(ros2_control_it, kNameAttribute, kROS2ControlTag);
hardware.type = get_attribute_value(ros2_control_it, kTypeAttribute, kROS2ControlTag);
hardware.rw_rate = parse_rw_rate_attribute(ros2_control_it);
hardware.rw_rate = parse_rw_rate_attribute(ros2_control_it, cm_update_rate);
hardware.is_async = parse_is_async_attribute(ros2_control_it);

// Parse everything under ros2_control tag
@@ -833,7 +836,8 @@ void update_interface_limits(

} // namespace detail

std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string & urdf)
std::vector<HardwareInfo> parse_control_resources_from_urdf(
const std::string & urdf, const unsigned int cm_update_rate)
{
// Check if everything OK with URDF string
if (urdf.empty())
@@ -880,7 +884,7 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
std::vector<HardwareInfo> hardware_info;
while (ros2_control_it)
{
hardware_info.push_back(detail::parse_resource_from_xml(ros2_control_it, urdf));
hardware_info.push_back(detail::parse_resource_from_xml(ros2_control_it, urdf, cm_update_rate));
ros2_control_it = ros2_control_it->NextSiblingElement(kROS2ControlTag);
}

8 changes: 3 additions & 5 deletions hardware_interface/src/resource_manager.cpp
Original file line number Diff line number Diff line change
@@ -142,10 +142,7 @@ class ResourceStorage
component_info.name = hardware_info.name;
component_info.type = hardware_info.type;
component_info.group = hardware_info.group;
component_info.rw_rate =
(hardware_info.rw_rate == 0 || hardware_info.rw_rate > cm_update_rate_)
? cm_update_rate_
: hardware_info.rw_rate;
component_info.rw_rate = hardware_info.rw_rate;
component_info.plugin_name = hardware_info.hardware_plugin_name;
component_info.is_async = hardware_info.is_async;

@@ -1127,7 +1124,8 @@ bool ResourceManager::load_and_initialize_components(

resource_storage_->cm_update_rate_ = update_rate;

const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
const auto hardware_info =
hardware_interface::parse_control_resources_from_urdf(urdf, resource_storage_->cm_update_rate_);

const std::string system_type = "system";
const std::string sensor_type = "sensor";
4 changes: 3 additions & 1 deletion hardware_interface/test/test_component_parser.cpp
Original file line number Diff line number Diff line change
@@ -1418,13 +1418,14 @@ TEST_F(TestComponentParser, gripper_mimic_true_valid_config)
std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) +
std::string(ros2_control_test_assets::urdf_tail);
std::vector<hardware_interface::HardwareInfo> hw_info;
ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test));
ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test, 200));
ASSERT_THAT(hw_info, SizeIs(1));
ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(1));
EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 2.0);
EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0);
EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0);
EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1);
EXPECT_EQ(hw_info[0].rw_rate, 200);
}

TEST_F(TestComponentParser, gripper_no_mimic_valid_config)
@@ -1441,6 +1442,7 @@ TEST_F(TestComponentParser, gripper_no_mimic_valid_config)
EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0);
EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0);
EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1);
EXPECT_EQ(hw_info[0].rw_rate, 100);
}

TEST_F(TestComponentParser, negative_rw_rates_throws_error)

0 comments on commit a386668

Please sign in to comment.