Skip to content

Commit

Permalink
make possible to export only subset of command/state interfaces, fix
Browse files Browse the repository at this point in the history 
adding node multiple times to executor
  • Loading branch information
@mamueluth
mamueluth committed May 17, 2023
1 parent 9764f8f commit 8b4b553
Show file tree
Hide file tree
Showing 2 changed files with 136 additions and 54 deletions.
4 changes: 4 additions & 0 deletions controller_manager/include/controller_manager/controller_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -460,6 +460,10 @@ class ControllerManager : public rclcpp::Node
bool distributed_ = false;
bool sub_controller_manager_ = false;
bool use_multiple_nodes_ = false;
// TODO(Manuel): weak_ptr would probably be a better choice. This way has to be checked
// if pointer points to an object. Don't like the nullptr thing and implicit checks
// associated with it ... (create on distributed Handles and StatePublisher/CommandForwarder)
// needs to be checked if is nullptr before usage
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> distributed_pub_sub_node_ = nullptr;
std::chrono::milliseconds distributed_interfaces_publish_period_ = std::chrono::milliseconds(12);

Expand Down
186 changes: 132 additions & 54 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -403,7 +403,6 @@ void ControllerManager::configure_controller_manager(const controller_manager_ty
case controller_manager_type::distributed_central_controller_manager:
init_distributed_central_controller_manager();
break;

case controller_manager_type::distributed_sub_controller_manager:
init_distributed_sub_controller_manager();
break;
Expand All @@ -419,7 +418,7 @@ void ControllerManager::configure_controller_manager(const controller_manager_ty
}
}

// TODO(Manuel) don't like this, this is for fast poc
// TODO(Manuel): don't like this, this is for fast poc
// probably better to create factory and handle creation of correct controller manager type
// there. Since asynchronous control should be supported im the future as well and we don't
// want dozen of ifs.
Expand Down Expand Up @@ -447,12 +446,27 @@ ControllerManager::controller_manager_type ControllerManager::determine_controll

void ControllerManager::init_distributed_sub_controller_manager()
{
// if only one node per sub controller manager is used
if (!use_multiple_nodes())
{
// create node for publishing/subscribing
rclcpp::NodeOptions node_options;
distributed_pub_sub_node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
std::string(get_name()) + "_pub_sub_node", get_namespace(), node_options, false);
//try to add to executor
try
{
executor_->add_node(distributed_pub_sub_node_->get_node_base_interface());
}
catch (const std::runtime_error & e)
{
RCLCPP_WARN_STREAM(
get_logger(),
"ControllerManager: can not add node for distributed publishing/subscribing to executor:"
<< e.what());
}
}

create_hardware_state_publishers();
create_hardware_command_forwarders();
register_sub_controller_manager();
Expand All @@ -465,6 +479,18 @@ void ControllerManager::init_distributed_central_controller_manager()
rclcpp::NodeOptions node_options;
distributed_pub_sub_node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
std::string(get_name()) + "_pub_sub_node", get_namespace(), node_options, false);
//try to add to executor
try
{
executor_->add_node(distributed_pub_sub_node_->get_node_base_interface());
}
catch (const std::runtime_error & e)
{
RCLCPP_WARN_STREAM(
get_logger(),
"ControllerManager: can not add node for distributed publishing/subscribing to executor:"
<< e.what());
}
}
init_distributed_central_controller_manager_services();
}
Expand Down Expand Up @@ -499,54 +525,66 @@ void ControllerManager::register_sub_controller_manager_srv_cb(
std::vector<std::shared_ptr<hardware_interface::DistributedReadOnlyHandle>>
distributed_state_interfaces;
distributed_state_interfaces.reserve(sub_ctrl_mng_wrapper->get_state_publisher_count());
// create distributed state interface and import into resource storage.
distributed_state_interfaces =
resource_manager_->import_state_interfaces_of_sub_controller_manager(
sub_ctrl_mng_wrapper, get_namespace(), distributed_pub_sub_node_);

for (const auto & state_interface : distributed_state_interfaces)
// register every node of state_interface at executor only if multiple nodes
// are used. Otherwise the single nodes has already been added
if (use_multiple_nodes())
{
try
{
executor_->add_node(state_interface->get_node()->get_node_base_interface());
}
catch (const std::runtime_error & e)
for (const auto & state_interface : distributed_state_interfaces)
{
response->ok = false;
RCLCPP_WARN_STREAM(
get_logger(),
"ControllerManager: Caught exception while trying to register sub controller manager. "
"Exception:"
<< e.what());
try
{
executor_->add_node(state_interface->get_node()->get_node_base_interface());
}
catch (const std::runtime_error & e)
{
response->ok = false;
RCLCPP_WARN_STREAM(
get_logger(),
"ControllerManager: Caught exception while trying to register node of distributed state "
"interface of sub controller manager. Exception:"
<< e.what());
}
}
}

std::vector<std::shared_ptr<hardware_interface::DistributedReadWriteHandle>>
distributed_command_interfaces;
distributed_command_interfaces.reserve(sub_ctrl_mng_wrapper->get_command_forwarder_count());
// create distributed command interface and import into resource storage.
distributed_command_interfaces =
resource_manager_->import_command_interfaces_of_sub_controller_manager(
sub_ctrl_mng_wrapper, get_namespace(), distributed_pub_sub_node_);

for (const auto & command_interface : distributed_command_interfaces)
{
try
{
executor_->add_node(command_interface->get_node()->get_node_base_interface());
}
catch (const std::runtime_error & e)
// register every node of command_interface at executor only if multiple nodes
// are used. Otherwise the single nodes has already been added
if (use_multiple_nodes())
{
response->ok = false;
RCLCPP_WARN_STREAM(
get_logger(),
"ControllerManager: Caught exception while trying to register sub controller manager. "
"Exception:"
<< e.what());
try
{
executor_->add_node(command_interface->get_node()->get_node_base_interface());
}
catch (const std::runtime_error & e)
{
response->ok = false;
RCLCPP_WARN_STREAM(
get_logger(),
"ControllerManager: Caught exception while trying to register node of distributed "
"command_interface of sub controller manager. Exception:"
<< e.what());
}
}
auto msg = controller_manager_msgs::msg::PublisherDescription();
msg.ns = get_namespace();
msg.name.prefix_name = command_interface->get_prefix_name();
msg.name.interface_name = command_interface->get_interface_name();
// TODO(Manuel) want topic name relative to namespace, but have to treat "root" namespace separate
// TODO(Manuel): want topic name relative to namespace, but have to treat "root" namespace separate
msg.publisher_topic = std::string("/") + command_interface->forward_command_topic_name();
response->command_state_publishers.push_back(msg);
}
Expand All @@ -559,52 +597,92 @@ void ControllerManager::register_sub_controller_manager_srv_cb(

void ControllerManager::create_hardware_state_publishers()
{
auto available_state_interfaces = resource_manager_->available_state_interfaces();

for (const auto & state_interface : available_state_interfaces)
std::vector<std::string> state_interfaces_to_export = std::vector<std::string>({});
// export every interface by default
if (!get_parameter("export_state_interfaces", state_interfaces_to_export))
{
auto state_publisher = std::make_shared<distributed_control::StatePublisher>(
std::move(std::make_unique<hardware_interface::LoanedStateInterface>(
resource_manager_->claim_state_interface(state_interface))),
get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_);

resource_manager_->add_hardware_state_publishers(state_publisher);
// get all available state interfaces
state_interfaces_to_export = resource_manager_->available_state_interfaces();
}

for (const auto & state_interface : state_interfaces_to_export)
{
std::shared_ptr<distributed_control::StatePublisher> state_publisher;
try
{
executor_->add_node(state_publisher->get_node()->get_node_base_interface());
state_publisher = std::make_shared<distributed_control::StatePublisher>(
std::move(std::make_unique<hardware_interface::LoanedStateInterface>(
resource_manager_->claim_state_interface(state_interface))),
get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_);
}
catch (const std::runtime_error & e)
catch (const std::exception & e)
{
RCLCPP_WARN_STREAM(
get_logger(), "ControllerManager: Can't create StatePublishers<"
<< state_publisher->state_interface_name() << ">." << e.what());
RCLCPP_ERROR(
get_logger(), "Can't create StatePublisher for state interface<'%s'>: %s",
state_interface.c_str(), e.what());
continue;
}

resource_manager_->add_hardware_state_publishers(state_publisher);

if (use_multiple_nodes())
{
try
{
executor_->add_node(state_publisher->get_node()->get_node_base_interface());
}
catch (const std::runtime_error & e)
{
RCLCPP_WARN_STREAM(
get_logger(), "ControllerManager: Can't create StatePublishers<"
<< state_publisher->state_interface_name() << ">." << e.what());
}
}
}
}

void ControllerManager::create_hardware_command_forwarders()
{
auto available_command_interfaces = resource_manager_->available_command_interfaces();

for (auto const & command_interface : available_command_interfaces)
std::vector<std::string> command_interfaces_to_export = std::vector<std::string>({});
// export every interface by default
if (!get_parameter("export_command_interfaces", command_interfaces_to_export))
{
auto command_forwarder = std::make_shared<distributed_control::CommandForwarder>(
std::move(std::make_unique<hardware_interface::LoanedCommandInterface>(
resource_manager_->claim_command_interface(command_interface))),
get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_);

resource_manager_->add_hardware_command_forwarders(command_forwarder);
// get all available command interfaces
command_interfaces_to_export = resource_manager_->available_command_interfaces();
}

for (auto const & command_interface : command_interfaces_to_export)
{
std::shared_ptr<distributed_control::CommandForwarder> command_forwarder;
try
{
executor_->add_node(command_forwarder->get_node()->get_node_base_interface());
command_forwarder = std::make_shared<distributed_control::CommandForwarder>(
std::move(std::make_unique<hardware_interface::LoanedCommandInterface>(
resource_manager_->claim_command_interface(command_interface))),
get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_);
}
catch (const std::runtime_error & e)
catch (const std::exception & e)
{
RCLCPP_WARN_STREAM(
get_logger(), "ControllerManager: Can't create CommandForwarder<"
<< command_forwarder->command_interface_name() << ">." << e.what());
RCLCPP_ERROR(
get_logger(), "Can't create CommandForwarder for command interface<'%s'>: %s",
command_interface.c_str(), e.what());
continue;
}

resource_manager_->add_hardware_command_forwarders(command_forwarder);

if (use_multiple_nodes())
{
try
{
executor_->add_node(command_forwarder->get_node()->get_node_base_interface());
}
catch (const std::runtime_error & e)
{
RCLCPP_WARN_STREAM(
get_logger(), "ControllerManager: Can't create CommandForwarder<"
<< command_forwarder->command_interface_name() << ">." << e.what());
}
}
}
}
Expand Down

0 comments on commit 8b4b553

Please sign in to comment.