diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 036370a9a9..0b0bccbd2a 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -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 distributed_pub_sub_node_ = nullptr; std::chrono::milliseconds distributed_interfaces_publish_period_ = std::chrono::milliseconds(12); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 567b8e291e..e28d970b29 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -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; @@ -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. @@ -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( 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(); @@ -465,6 +479,18 @@ void ControllerManager::init_distributed_central_controller_manager() rclcpp::NodeOptions node_options; distributed_pub_sub_node_ = std::make_shared( 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(); } @@ -499,54 +525,66 @@ void ControllerManager::register_sub_controller_manager_srv_cb( std::vector> 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> 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); } @@ -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 state_interfaces_to_export = std::vector({}); + // export every interface by default + if (!get_parameter("export_state_interfaces", state_interfaces_to_export)) { - auto state_publisher = std::make_shared( - std::move(std::make_unique( - 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 state_publisher; try { - executor_->add_node(state_publisher->get_node()->get_node_base_interface()); + state_publisher = std::make_shared( + std::move(std::make_unique( + 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 command_interfaces_to_export = std::vector({}); + // export every interface by default + if (!get_parameter("export_command_interfaces", command_interfaces_to_export)) { - auto command_forwarder = std::make_shared( - std::move(std::make_unique( - 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 command_forwarder; try { - executor_->add_node(command_forwarder->get_node()->get_node_base_interface()); + command_forwarder = std::make_shared( + std::move(std::make_unique( + 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()); + } } } }