Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
16 changes: 13 additions & 3 deletions hardware_interface/include/hardware_interface/hardware_info.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -260,6 +260,13 @@ struct HardwareAsyncParams
};

/// This structure stores information about hardware defined in a robot's URDF.
#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4996)
#else
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
struct HardwareInfo
{
/// Name of the hardware.
Expand All @@ -272,10 +279,8 @@ struct HardwareInfo
unsigned int rw_rate;
/// Component is async
bool is_async;
// TODO(anyone): deprecate after branching for kilted
/// [[deprecated("Use async_params instead.")]]
/// Async thread priority
int thread_priority;
[[deprecated("Use async_params instead.")]] int thread_priority;
/// Async Parameters
HardwareAsyncParams async_params;
/// Name of the pluginlib plugin of the hardware that will be loaded.
Expand Down Expand Up @@ -322,6 +327,11 @@ struct HardwareInfo
*/
std::unordered_map<std::string, joint_limits::SoftJointLimits> soft_limits;
};
#ifdef _MSC_VER
#pragma warning(pop)
#else
#pragma GCC diagnostic pop
#endif

} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__HARDWARE_INFO_HPP_
33 changes: 30 additions & 3 deletions hardware_interface/src/component_parser.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -675,9 +675,23 @@ HardwareInfo parse_resource_from_xml(
hardware.type = get_attribute_value(ros2_control_it, kTypeAttribute, kROS2ControlTag);
hardware.rw_rate = parse_rw_rate_attribute(ros2_control_it);
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();
hardware.async_params.thread_priority = hardware.thread_priority;
hardware.async_params.thread_priority = hardware.is_async
? parse_thread_priority_attribute(ros2_control_it)
: std::numeric_limits<int>::max();
// TODO(anyone): remove this line once thread_priority is removed
#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4996)
#else
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
hardware.thread_priority = hardware.async_params.thread_priority;
#ifdef _MSC_VER
#pragma warning(pop)
#else
#pragma GCC diagnostic pop
#endif

// Parse everything under ros2_control tag
hardware.hardware_plugin_name = "";
Expand Down Expand Up @@ -725,7 +739,20 @@ HardwareInfo parse_resource_from_xml(
if (async_it->FindAttribute(kThreadPriorityAttribute))
{
hardware.async_params.thread_priority = parse_thread_priority_attribute(async_it);
// TODO(anyone): remove this line once thread_priority is removed
#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4996)
#else
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
hardware.thread_priority = hardware.async_params.thread_priority;
#ifdef _MSC_VER
#pragma warning(pop)
#else
#pragma GCC diagnostic pop
#endif
}
if (async_it->FindAttribute(kPrintWarningsAttribute))
{
Expand Down
93 changes: 87 additions & 6 deletions hardware_interface/test/test_component_parser.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -880,7 +880,21 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio
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());
// TODO(anyone): remove this line once thread_priority is removed
#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4996)
#else
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
ASSERT_EQ(hardware_info.thread_priority, hardware_info.async_params.thread_priority);
#ifdef _MSC_VER
#pragma warning(pop)
#else
#pragma GCC diagnostic pop
#endif
ASSERT_EQ(hardware_info.async_params.thread_priority, std::numeric_limits<int>::max());
ASSERT_THAT(hardware_info.joints, SizeIs(2));

EXPECT_EQ(hardware_info.joints[0].name, "joint1");
Expand Down Expand Up @@ -952,7 +966,21 @@ 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());
// TODO(anyone): remove this line once thread_priority is removed
#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4996)
#else
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
ASSERT_EQ(hardware_info.thread_priority, hardware_info.async_params.thread_priority);
#ifdef _MSC_VER
#pragma warning(pop)
#else
#pragma GCC diagnostic pop
#endif
ASSERT_EQ(hardware_info.async_params.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));
Expand Down Expand Up @@ -1002,7 +1030,21 @@ TEST_F(

ASSERT_THAT(hardware_info.joints, SizeIs(1));
ASSERT_FALSE(hardware_info.is_async);
ASSERT_EQ(hardware_info.thread_priority, std::numeric_limits<int>::max());
// TODO(anyone): remove this line once thread_priority is removed
#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4996)
#else
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
ASSERT_EQ(hardware_info.thread_priority, hardware_info.async_params.thread_priority);
#ifdef _MSC_VER
#pragma warning(pop)
#else
#pragma GCC diagnostic pop
#endif
ASSERT_EQ(hardware_info.async_params.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(2));
Expand Down Expand Up @@ -1430,7 +1472,20 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_components)
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);
// TODO(anyone): remove this line once thread_priority is removed
#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4996)
#else
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
ASSERT_EQ(hardware_info.thread_priority, hardware_info.async_params.thread_priority);
#ifdef _MSC_VER
#pragma warning(pop)
#else
#pragma GCC diagnostic pop
#endif
ASSERT_EQ(hardware_info.async_params.thread_priority, 30);
ASSERT_EQ(hardware_info.async_params.scheduling_policy, "detached");
ASSERT_FALSE(hardware_info.async_params.print_warnings);
Expand All @@ -1450,7 +1505,20 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_components)
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);
// TODO(anyone): remove this line once thread_priority is removed
#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4996)
#else
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
ASSERT_EQ(hardware_info.thread_priority, hardware_info.async_params.thread_priority);
#ifdef _MSC_VER
#pragma warning(pop)
#else
#pragma GCC diagnostic pop
#endif
ASSERT_EQ(hardware_info.async_params.thread_priority, 50);
ASSERT_EQ(hardware_info.async_params.scheduling_policy, "synchronized");
ASSERT_TRUE(hardware_info.async_params.print_warnings);
Expand Down Expand Up @@ -1478,7 +1546,20 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_components)
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);
// TODO(anyone): remove this line once thread_priority is removed
#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4996)
#else
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
ASSERT_EQ(hardware_info.thread_priority, hardware_info.async_params.thread_priority);
#ifdef _MSC_VER
#pragma warning(pop)
#else
#pragma GCC diagnostic pop
#endif
ASSERT_EQ(hardware_info.async_params.thread_priority, 70);
ASSERT_EQ(hardware_info.async_params.scheduling_policy, "synchronized");
ASSERT_EQ(1u, hardware_info.async_params.cpu_affinity_cores.size());
Expand Down
Loading