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
13 changes: 1 addition & 12 deletions controller_interface/src/controller_interface_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,6 @@ return_type ControllerInterfaceBase::init(
// no rclcpp::ParameterValue unsigned int specialization
auto_declare<int>("update_rate", static_cast<int>(params.controller_manager_update_rate));
auto_declare<bool>("is_async", false);
auto_declare<int>("thread_priority", -100);
}
catch (const std::exception & e)
{
Expand Down Expand Up @@ -246,17 +245,7 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
{
realtime_tools::AsyncFunctionHandlerParams async_params;
async_params.thread_priority = 50; // default value
const int thread_priority_param =
static_cast<int>(get_node()->get_parameter("thread_priority").as_int());
if (thread_priority_param >= 0 && thread_priority_param <= 99)
{
async_params.thread_priority = thread_priority_param;
RCLCPP_WARN(
get_node()->get_logger(),
"The parsed 'thread_priority' parameter will be deprecated and not be functional from "
"ROS 2 Lyrical Luth release. Please use the 'async_parameters.thread_priority' parameter "
"instead.");
}

async_params.initialize(impl_->node_, "async_parameters.");
if (async_params.scheduling_policy == realtime_tools::AsyncSchedulingPolicy::DETACHED)
{
Expand Down
14 changes: 0 additions & 14 deletions hardware_interface/include/hardware_interface/hardware_info.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -359,13 +359,6 @@ 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 @@ -378,8 +371,6 @@ struct HardwareInfo
unsigned int rw_rate;
/// Component is async
bool is_async;
/// Async 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 @@ -426,11 +417,6 @@ 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_
63 changes: 21 additions & 42 deletions hardware_interface/src/component_parser.cpp
Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

where do these changes come from?

Please revise the changes, we only want to remove the deprecated variable and its usage.

Copy link
Copy Markdown
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

will changes be made to hardware_interface/include/hardware_interface/hardware_info.hpp only ? presence of thread_priority in rest files remain unchanged?

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I suggest removing the member variable, and go through the build errors where this variable was used ;) Please remove all the deprecation-silencing preprocessor macros added with #2644

Original file line number Diff line number Diff line change
Expand Up @@ -117,16 +117,8 @@ std::string get_attribute_value(
attr = element_it->FindAttribute(attribute_name);
if (!attr)
{
const char * name = element_it->Attribute(kNameAttribute);
if (name && std::strlen(name) > 0)
{
throw std::runtime_error(
fmt::format(
FMT_COMPILE("no attribute '{}' in '{}' tag with name '{}'"), attribute_name, tag_name,
name));
}
throw std::runtime_error(
fmt::format(FMT_COMPILE("no attribute '{}' in '{}' tag"), attribute_name, tag_name));
fmt::format(FMT_COMPILE("no attribute {} in {} tag"), attribute_name, tag_name));
}
return ros2_control::strip(element_it->Attribute(attribute_name));
}
Expand Down Expand Up @@ -345,7 +337,7 @@ int parse_thread_priority_attribute(const tinyxml2::XMLElement * elem)
* \throws std::runtime_error if a component attribute or tag is not found
*/
std::unordered_map<std::string, std::string> parse_parameters_from_xml(
const tinyxml2::XMLElement * params_it, const std::string & context_name)
const tinyxml2::XMLElement * params_it)
{
std::unordered_map<std::string, std::string> parameters;
const tinyxml2::XMLAttribute * attr;
Expand All @@ -356,10 +348,7 @@ std::unordered_map<std::string, std::string> parse_parameters_from_xml(
attr = params_it->FindAttribute(kNameAttribute);
if (!attr)
{
throw std::runtime_error(
fmt::format(
FMT_COMPILE("no parameter name attribute set in param tag{}"),
context_name.empty() ? "" : " for '" + context_name + "'"));
throw std::runtime_error("no parameter name attribute set in param tag");
}
const std::string parameter_name = ros2_control::strip(params_it->Attribute(kNameAttribute));
const std::string parameter_value =
Expand All @@ -379,7 +368,7 @@ std::unordered_map<std::string, std::string> parse_parameters_from_xml(
* \throws std::runtime_error if the interfaceType text not set in a tag
*/
hardware_interface::InterfaceInfo parse_interfaces_from_xml(
const tinyxml2::XMLElement * interfaces_it, const std::string & context_name)
const tinyxml2::XMLElement * interfaces_it)
{
hardware_interface::InterfaceInfo interface;

Expand All @@ -389,7 +378,7 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml(

// Optional min/max attributes
std::unordered_map<std::string, std::string> interface_params =
parse_parameters_from_xml(interfaces_it->FirstChildElement(kParamTag), context_name);
parse_parameters_from_xml(interfaces_it->FirstChildElement(kParamTag));
auto interface_param = interface_params.find(kMinTag);
if (interface_param != interface_params.end())
{
Expand Down Expand Up @@ -425,7 +414,7 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml(
const auto * params_it = interfaces_it->FirstChildElement(kParamTag);
if (params_it)
{
interface.parameters = parse_parameters_from_xml(params_it, context_name);
interface.parameters = parse_parameters_from_xml(params_it);
}

interface.data_type = parse_data_type_attribute(interfaces_it);
Expand Down Expand Up @@ -477,7 +466,7 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it
const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTag);
while (command_interfaces_it)
{
InterfaceInfo cmd_info = parse_interfaces_from_xml(command_interfaces_it, component.name);
InterfaceInfo cmd_info = parse_interfaces_from_xml(command_interfaces_it);
cmd_info.enable_limits &= component.enable_limits;
component.command_interfaces.push_back(cmd_info);
command_interfaces_it = command_interfaces_it->NextSiblingElement(kCommandInterfaceTag);
Expand All @@ -487,7 +476,7 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it
const auto * state_interfaces_it = component_it->FirstChildElement(kStateInterfaceTag);
while (state_interfaces_it)
{
InterfaceInfo state_info = parse_interfaces_from_xml(state_interfaces_it, component.name);
InterfaceInfo state_info = parse_interfaces_from_xml(state_interfaces_it);
state_info.enable_limits &= component.enable_limits;
component.state_interfaces.push_back(state_info);
state_interfaces_it = state_interfaces_it->NextSiblingElement(kStateInterfaceTag);
Expand All @@ -497,7 +486,7 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it
const auto * params_it = component_it->FirstChildElement(kParamTag);
if (params_it)
{
component.parameters = parse_parameters_from_xml(params_it, component.name);
component.parameters = parse_parameters_from_xml(params_it);
}

return component;
Expand All @@ -524,25 +513,23 @@ ComponentInfo parse_complex_component_from_xml(const tinyxml2::XMLElement * comp
const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTag);
while (command_interfaces_it)
{
component.command_interfaces.push_back(
parse_interfaces_from_xml(command_interfaces_it, component.name));
component.command_interfaces.push_back(parse_interfaces_from_xml(command_interfaces_it));
command_interfaces_it = command_interfaces_it->NextSiblingElement(kCommandInterfaceTag);
}

// Parse state interfaces
const auto * state_interfaces_it = component_it->FirstChildElement(kStateInterfaceTag);
while (state_interfaces_it)
{
component.state_interfaces.push_back(
parse_interfaces_from_xml(state_interfaces_it, component.name));
component.state_interfaces.push_back(parse_interfaces_from_xml(state_interfaces_it));
state_interfaces_it = state_interfaces_it->NextSiblingElement(kStateInterfaceTag);
}

// Parse parameters
const auto * params_it = component_it->FirstChildElement(kParamTag);
if (params_it)
{
component.parameters = parse_parameters_from_xml(params_it, component.name);
component.parameters = parse_parameters_from_xml(params_it);
}

return component;
Expand Down Expand Up @@ -587,10 +574,7 @@ TransmissionInfo parse_transmission_from_xml(const tinyxml2::XMLElement * transm
const auto * type_it = transmission_it->FirstChildElement(kPluginNameTag);
if (!type_it)
{
throw std::runtime_error(
fmt::format(
FMT_COMPILE("Missing <plugin> tag of <transmission> element for '{}' in your URDF."),
transmission.name));
throw std::runtime_error("Missing <plugin> tag of <transmission> element in your URDF.");
}
transmission.type = get_text_for_element(type_it, kPluginNameTag);

Expand All @@ -614,7 +598,7 @@ TransmissionInfo parse_transmission_from_xml(const tinyxml2::XMLElement * transm
const auto * params_it = transmission_it->FirstChildElement(kParamTag);
if (params_it)
{
transmission.parameters = parse_parameters_from_xml(params_it, transmission.name);
transmission.parameters = parse_parameters_from_xml(params_it);
}

return transmission;
Expand Down Expand Up @@ -694,9 +678,8 @@ 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.async_params.thread_priority = hardware.is_async
? parse_thread_priority_attribute(ros2_control_it)
: std::numeric_limits<int>::max();
hardware.async_params.thread_priority = 50;

// TODO(anyone): remove this line once thread_priority is removed
#ifdef _MSC_VER
#pragma warning(push)
Expand All @@ -705,7 +688,7 @@ HardwareInfo parse_resource_from_xml(
#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
Expand All @@ -722,10 +705,7 @@ HardwareInfo parse_resource_from_xml(
const auto * type_it = ros2_control_child_it->FirstChildElement(kPluginNameTag);
if (!type_it)
{
throw std::runtime_error(
fmt::format(
FMT_COMPILE("Missing <plugin> tag of <hardware> element for '{}' in your URDF."),
hardware.name));
throw std::runtime_error("Missing <plugin> tag of <hardware> element in your URDF.");
}
hardware.hardware_plugin_name =
get_text_for_element(type_it, std::string("hardware ") + kPluginNameTag);
Expand All @@ -737,7 +717,7 @@ HardwareInfo parse_resource_from_xml(
const auto * params_it = ros2_control_child_it->FirstChildElement(kParamTag);
if (params_it)
{
hardware.hardware_parameters = parse_parameters_from_xml(params_it, hardware.name);
hardware.hardware_parameters = parse_parameters_from_xml(params_it);
}
}
else if (std::string(kPropertiesTag) == ros2_control_child_it->Name())
Expand Down Expand Up @@ -769,7 +749,7 @@ HardwareInfo parse_resource_from_xml(
#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
Expand Down Expand Up @@ -1033,8 +1013,7 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &

if (!ros2_control_it)
{
throw std::runtime_error(
fmt::format(FMT_COMPILE("no '{}' tag found in the URDF"), kROS2ControlTag));
throw std::runtime_error(fmt::format(FMT_COMPILE("no {} tag"), kROS2ControlTag));
}

std::vector<HardwareInfo> hardware_info;
Expand Down
Loading