parse thread priority from the URDF ros2_control component

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -137,6 +137,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
@@ -567,6 +597,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

@@ -777,6 +777,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");
@@ -847,6 +849,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));
@@ -1235,6 +1239,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 =
@@ -1275,6 +1332,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>