parse thread priority from the URDF ros2_control component

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.

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<int>::max();
 
   // Parse everything under ros2_control tag
   hardware.hardware_plugin_name = "";

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<int>::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<int>::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) +
+                             "<ros2_control name='TestActuatorHardware' type='actuator' "
+                             "is_async='true' thread_priority='-30'/>" +
+                             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) +

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"(
-  <ros2_control name="TestActuatorHardware" type="actuator" is_async="true">
+  <ros2_control name="TestActuatorHardware" type="actuator" is_async="true" thread_priority="30">
     <hardware>
       <plugin>test_actuator</plugin>
     </hardware>
@@ -683,7 +683,7 @@ const auto async_hardware_resources =
       <state_interface name="velocity"/>
     </sensor>
   </ros2_control>
-  <ros2_control name="TestSystemHardware" type="system" is_async="true">
+  <ros2_control name="TestSystemHardware" type="system" is_async="true" thread_priority="70">
     <hardware>
       <plugin>test_system</plugin>
       <param name="example_param_write_for_sec">2</param>