diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index eea8b6ca8a..7246cc6306 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -176,6 +176,8 @@ struct HardwareInfo std::string group; /// Component is async bool is_async; + /// Async thread priority + int thread_priority; /// Name of the pluginlib plugin of the hardware that will be loaded. std::string hardware_plugin_name; /// (Optional) Key-value pairs for hardware parameters. diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index d2ec0f9d53..9bb90df9e1 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -57,6 +57,7 @@ constexpr const auto kRoleAttribute = "role"; constexpr const auto kReductionAttribute = "mechanical_reduction"; constexpr const auto kOffsetAttribute = "offset"; constexpr const auto kIsAsyncAttribute = "is_async"; +constexpr const auto kThreadPriorityAttribute = "thread_priority"; } // namespace @@ -234,6 +235,35 @@ bool parse_is_async_attribute(const tinyxml2::XMLElement * elem) return attr ? parse_bool(attr->Value()) : false; } +/// Parse thread_priority attribute +/** + * Parses an XMLElement and returns the value of the thread_priority attribute. + * Defaults to 50 if not specified. + * + * \param[in] elem XMLElement that has the thread_priority attribute. + * \return positive integer specifying the thread priority. + */ +int parse_thread_priority_attribute(const tinyxml2::XMLElement * elem) +{ + const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kThreadPriorityAttribute); + if (!attr) + { + return 50; + } + std::string s = attr->Value(); + std::regex int_re("[1-9][0-9]*"); + if (std::regex_match(s, int_re)) + { + return std::stoi(s); + } + else + { + throw std::runtime_error( + "Could not parse thread_priority tag in \"" + std::string(elem->Name()) + "\"." + "Got \"" + + s + "\", but expected a non-zero positive integer."); + } +} + /// Search XML snippet from URDF for parameters. /** * \param[in] params_it pointer to the iterator where parameters info should be found @@ -574,6 +604,8 @@ HardwareInfo parse_resource_from_xml( hardware.name = get_attribute_value(ros2_control_it, kNameAttribute, kROS2ControlTag); hardware.type = get_attribute_value(ros2_control_it, kTypeAttribute, kROS2ControlTag); hardware.is_async = parse_is_async_attribute(ros2_control_it); + hardware.thread_priority = hardware.is_async ? parse_thread_priority_attribute(ros2_control_it) + : std::numeric_limits::max(); // Parse everything under ros2_control tag hardware.hardware_plugin_name = ""; diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 20e098b62e..b8a90836fe 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -879,6 +879,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware"); + ASSERT_FALSE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, std::numeric_limits::max()); ASSERT_THAT(hardware_info.joints, SizeIs(2)); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); @@ -949,6 +951,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_d ASSERT_THAT(hardware_info.joints, SizeIs(1)); + ASSERT_FALSE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, std::numeric_limits::max()); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); EXPECT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); @@ -1337,6 +1341,59 @@ TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_with_limit EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits); } +TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_components) +{ + std::string urdf_to_test = ros2_control_test_assets::minimal_async_robot_urdf; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(3)); + auto hardware_info = control_hardware[0]; + + // Actuator + EXPECT_EQ(hardware_info.name, "TestActuatorHardware"); + EXPECT_EQ(hardware_info.type, "actuator"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + ASSERT_THAT(hardware_info.joints, SizeIs(1)); + ASSERT_TRUE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, 30); + + EXPECT_EQ(hardware_info.joints[0].name, "joint1"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + + // Sensor + hardware_info = control_hardware[1]; + EXPECT_EQ(hardware_info.name, "TestSensorHardware"); + EXPECT_EQ(hardware_info.type, "sensor"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + ASSERT_THAT(hardware_info.joints, IsEmpty()); + ASSERT_THAT(hardware_info.sensors, SizeIs(1)); + ASSERT_TRUE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, 50); + + EXPECT_EQ(hardware_info.sensors[0].name, "sensor1"); + EXPECT_EQ(hardware_info.sensors[0].type, "sensor"); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces, SizeIs(1)); + EXPECT_THAT(hardware_info.sensors[0].command_interfaces, IsEmpty()); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces[0].name, "velocity"); + + // System + hardware_info = control_hardware[2]; + EXPECT_EQ(hardware_info.name, "TestSystemHardware"); + EXPECT_EQ(hardware_info.type, "system"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + ASSERT_THAT(hardware_info.gpios, SizeIs(1)); + + EXPECT_EQ(hardware_info.joints[0].name, "joint2"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + + EXPECT_EQ(hardware_info.joints[1].name, "joint3"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + EXPECT_EQ(hardware_info.gpios[0].name, "configuration"); + EXPECT_EQ(hardware_info.gpios[0].type, "gpio"); + ASSERT_TRUE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, 70); +} + TEST_F(TestComponentParser, successfully_parse_parameter_empty) { const std::string urdf_to_test = @@ -1377,6 +1434,16 @@ TEST_F(TestComponentParser, successfully_parse_parameter_empty) } } +TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_invalid_thread_priority) +{ + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + "" + + std::string(ros2_control_test_assets::urdf_tail); + ; + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + TEST_F(TestComponentParser, negative_size_throws_error) { std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index cc2b1798d4..fdc94f08a0 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -662,7 +662,7 @@ const auto hardware_resources = const auto async_hardware_resources = R"( - + test_actuator @@ -683,7 +683,7 @@ const auto async_hardware_resources = - + test_system 2