Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Propagate read/write rate to the HardwareInfo properly #1927

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Propagate read/write rate to the HardwareInfo properly #1927

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion hardware_interface/include/hardware_interface/component_parser.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
16 changes: 10 additions & 6 deletions hardware_interface/src/component_parser.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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())
Expand Down Expand Up @@ -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);
}

Expand Down
8 changes: 3 additions & 5 deletions hardware_interface/src/resource_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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";
Expand Down
4 changes: 3 additions & 1 deletion hardware_interface/test/test_component_parser.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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)
Expand Down
Loading