Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[HW interface] Resource manager publish all Command-/StateInterface values #1244

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from 4 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
6 changes: 5 additions & 1 deletion controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -258,7 +258,7 @@ ControllerManager::ControllerManager(
const std::string & namespace_, const rclcpp::NodeOptions & options)
: rclcpp::Node(manager_node_name, namespace_, options),
resource_manager_(std::make_unique<hardware_interface::ResourceManager>(
update_rate_, this->get_node_clock_interface())),
update_rate_, this->get_node_clock_interface(), get_fully_qualified_name())),
diagnostics_updater_(this),
executor_(executor),
loader_(std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
Expand All @@ -267,6 +267,8 @@ ControllerManager::ControllerManager(
std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
{
executor_->add_node(resource_manager_->get_publisher_node());

if (!get_parameter("update_rate", update_rate_))
{
RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
Expand Down Expand Up @@ -308,6 +310,8 @@ ControllerManager::ControllerManager(
std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
{
executor_->add_node(resource_manager_->get_publisher_node());

if (!get_parameter("update_rate", update_rate_))
{
RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
Expand Down
1 change: 1 addition & 0 deletions hardware_interface/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
rclcpp_lifecycle
rcpputils
rcutils
realtime_tools
TinyXML2
tinyxml2_vendor
)
Expand Down
35 changes: 33 additions & 2 deletions hardware_interface/include/hardware_interface/resource_manager.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,8 @@
#include <unordered_map>
#include <vector>

#include "control_msgs/msg/dynamic_interface_values.hpp"
#include "control_msgs/msg/interface_value.hpp"
#include "hardware_interface/actuator.hpp"
#include "hardware_interface/hardware_component_info.hpp"
#include "hardware_interface/hardware_info.hpp"
Expand All @@ -33,6 +35,7 @@
#include "rclcpp/duration.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/time.hpp"
#include "realtime_tools/realtime_publisher.h"

namespace hardware_interface
{
Expand All @@ -51,7 +54,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
/// Default constructor for the Resource Manager.
ResourceManager(
unsigned int update_rate = 100,
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr,
const std::string & fully_qualified_ctrl_mng_node_name = "");

/// Constructor for the Resource Manager.
/**
Expand All @@ -71,12 +75,15 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
explicit ResourceManager(
const std::string & urdf, bool validate_interfaces = true, bool activate_all = false,
unsigned int update_rate = 100,
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr,
const std::string & fully_qualified_ctrl_mng_node_name = "");

ResourceManager(const ResourceManager &) = delete;

~ResourceManager();

rclcpp::Node::SharedPtr get_publisher_node() const;

/// Load resources from on a given URDF.
/**
* The resource manager can be post initialized with a given URDF.
Expand Down Expand Up @@ -400,6 +407,24 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
bool state_interface_exists(const std::string & key) const;

private:
/**
* We want to publish the values of all available State-/CommandInterfaces. In order to do this we
* need to have a node which publishes the values. This function creates a node relative to the
* ControllerManager's namespace and name, as well as a RealtimePublisher for publishing the
* values.
*
* \param[in] fully_qualified_ctrl_mng_node_name the full qualified name of the controller manager
* e.g. /namespace/controller_manager_1
*/
void create_interface_value_publisher(const std::string & fully_qualified_ctrl_mng_node_name);

/**
* Get all the values from the available State-/CommandInterface and publish them on the
* "~/interface_values" topic as diagnosis topic.
*
*/
void publish_all_interface_values() const;

void validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;

void release_command_interface(const std::string & key);
Expand All @@ -410,6 +435,12 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
mutable std::recursive_mutex claimed_command_interfaces_lock_;
mutable std::recursive_mutex resources_lock_;

rclcpp::Node::SharedPtr interface_value_publisher_node_;
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceValues>::SharedPtr
interface_values_publisher_;
std::unique_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicInterfaceValues>>
rt_interface_values_publisher_;

std::unique_ptr<ResourceStorage> resource_storage_;

// Structure to store read and write status so it is not initialized in the real-time loop
Expand Down
1 change: 1 addition & 0 deletions hardware_interface/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
<depend>pluginlib</depend>
<depend>rclcpp_lifecycle</depend>
<depend>rcpputils</depend>
<depend>realtime_tools</depend>
<depend>tinyxml2_vendor</depend>

<build_depend>rcutils</build_depend>
Expand Down
80 changes: 78 additions & 2 deletions hardware_interface/src/resource_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@
#include "lifecycle_msgs/msg/state.hpp"
#include "pluginlib/class_loader.hpp"
#include "rcutils/logging_macros.h"
#include "std_msgs/msg/header.h"

namespace hardware_interface
{
Expand Down Expand Up @@ -729,18 +730,22 @@ class ResourceStorage
};

ResourceManager::ResourceManager(
unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
const std::string & fully_qualified_ctrl_mng_node_name)
: resource_storage_(std::make_unique<ResourceStorage>(update_rate, clock_interface))
{
create_interface_value_publisher(fully_qualified_ctrl_mng_node_name);
}

ResourceManager::~ResourceManager() = default;

ResourceManager::ResourceManager(
const std::string & urdf, bool validate_interfaces, bool activate_all, unsigned int update_rate,
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
const std::string & fully_qualified_ctrl_mng_node_name)
: resource_storage_(std::make_unique<ResourceStorage>(update_rate, clock_interface))
{
create_interface_value_publisher(fully_qualified_ctrl_mng_node_name);
load_urdf(urdf, validate_interfaces);

if (activate_all)
Expand All @@ -754,6 +759,25 @@ ResourceManager::ResourceManager(
}
}

void ResourceManager::create_interface_value_publisher(
const std::string & fully_qualified_ctrl_mng_node_name)
{
rclcpp::NodeOptions options;
interface_value_publisher_node_ = rclcpp::Node::make_shared(
"resource_manager_publisher_node", fully_qualified_ctrl_mng_node_name, options);
interface_values_publisher_ =
interface_value_publisher_node_->create_publisher<control_msgs::msg::DynamicInterfaceValues>(
"~/interface_values", 10);
rt_interface_values_publisher_ =
std::make_unique<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicInterfaceValues>>(
interface_values_publisher_);
}

rclcpp::Node::SharedPtr ResourceManager::get_publisher_node() const
{
return interface_value_publisher_node_;
}

// CM API: Called in "callback/slow"-thread
void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces)
{
Expand Down Expand Up @@ -1283,6 +1307,56 @@ HardwareReadWriteStatus ResourceManager::read(
return read_write_status;
}

void ResourceManager::publish_all_interface_values() const
{
control_msgs::msg::DynamicInterfaceValues interface_values;
interface_values.header.stamp = resource_storage_->clock_interface_->get_clock()->now();
destogl marked this conversation as resolved.
Show resolved Hide resolved

control_msgs::msg::InterfaceValue state_interface_values;
for (const auto & state_interface_name : resource_storage_->available_state_interfaces_)
{
if (const auto iter = resource_storage_->state_interface_map_.find(state_interface_name);
iter != resource_storage_->state_interface_map_.end())
{
state_interface_values.interface_names.push_back(state_interface_name);
state_interface_values.values.push_back(iter->second.get_value());
}
else
{
RCUTILS_LOG_WARN_NAMED(
Copy link
Member

Choose a reason for hiding this comment

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

Out of curiosity, do you think this should be throttled? or do you think it is fine to have like this?

Copy link
Member Author

Choose a reason for hiding this comment

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

i am not sure about the performance penalty, but since this case should not happen under normal circumstances, i think its fine like this. But any thoughts/corrections are welcome.

"resource_manager",
"State interface '%s' is in available in state_interface_map_, but could not get the "
"interface",
state_interface_name.c_str());
}
}

control_msgs::msg::InterfaceValue command_interface_values;
for (const auto & command_interface_name : resource_storage_->available_command_interfaces_)
{
if (const auto iter = resource_storage_->command_interface_map_.find(command_interface_name);
iter != resource_storage_->command_interface_map_.end())
{
command_interface_values.interface_names.push_back(command_interface_name);
command_interface_values.values.push_back(iter->second.get_value());
}
else
{
RCUTILS_LOG_WARN_NAMED(
"resource_manager",
"command interface '%s' is available in command_interface_map_, but could not get the "
"interface",
command_interface_name.c_str());
}
}
interface_values.states = state_interface_values;
interface_values.commands = command_interface_values;
mamueluth marked this conversation as resolved.
Show resolved Hide resolved

rt_interface_values_publisher_->trylock();
rt_interface_values_publisher_->msg_ = interface_values;
rt_interface_values_publisher_->unlockAndPublish();
mamueluth marked this conversation as resolved.
Show resolved Hide resolved
}

// CM API: Called in "update"-thread
HardwareReadWriteStatus ResourceManager::write(
const rclcpp::Time & time, const rclcpp::Duration & period)
Expand All @@ -1291,6 +1365,8 @@ HardwareReadWriteStatus ResourceManager::write(
read_write_status.ok = true;
read_write_status.failed_hardware_names.clear();

publish_all_interface_values();

auto write_components = [&](auto & components)
{
for (auto & component : components)
Expand Down
Loading