From 84ec999f6566405d21b218480b2a98be0c1de26a Mon Sep 17 00:00:00 2001 From: brian Date: Fri, 19 Jul 2024 13:14:15 -0700 Subject: [PATCH 1/2] create individual contexts for fixing multi robot --- .../src/ign_ros2_control_plugin.cpp | 62 ++++++++++++++++++- 1 file changed, 59 insertions(+), 3 deletions(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index 7ebcfcb2..7d0e2c02 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -73,6 +73,9 @@ class IgnitionROS2ControlPluginPrivate /// \brief Thread where the executor will spin std::thread thread_executor_spin_; + /// \brief Context for the executor + rclcpp::Context::SharedPtr context_; + /// \brief Executor to spin the controller rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; @@ -293,11 +296,14 @@ void IgnitionROS2ControlPlugin::Configure( auto sdfPtr = const_cast(_sdf.get()); + std::vector parameter_file_names; + sdf::ElementPtr argument_sdf = sdfPtr->GetElement("parameters"); while (argument_sdf) { std::string argument = argument_sdf->Get(); arguments.push_back(RCL_PARAM_FILE_FLAG); arguments.push_back(argument); + parameter_file_names.push_back(argument); argument_sdf = argument_sdf->GetNextElement("parameters"); } @@ -340,6 +346,10 @@ void IgnitionROS2ControlPlugin::Configure( std::vector argv; for (const auto & arg : arguments) { + RCLCPP_INFO( + logger, + arg.data() + ); argv.push_back(reinterpret_cast(arg.data())); } @@ -348,10 +358,26 @@ void IgnitionROS2ControlPlugin::Configure( rclcpp::init(static_cast(argv.size()), argv.data()); } + // Create individual context + std::string msg = ns + " [gz_ros2_control]: rclcpp:init called"; + RCLCPP_INFO( + logger, + msg.data() + ); + + this->dataPtr->context_ = std::make_shared(); + this->dataPtr->context_->init(static_cast(argv.size()), argv.data()); + std::string node_name = "gz_ros2_control"; - this->dataPtr->node_ = rclcpp::Node::make_shared(node_name, ns); - this->dataPtr->executor_ = std::make_shared(); + rclcpp::ExecutorOptions executor_options; + executor_options.context = this->dataPtr->context_; + + rclcpp::NodeOptions node_options; + node_options.context(this->dataPtr->context_); + + this->dataPtr->node_ = rclcpp::Node::make_shared(node_name, ns, node_options); + this->dataPtr->executor_ = std::make_shared(executor_options); this->dataPtr->executor_->add_node(this->dataPtr->node_); auto spin = [this]() { @@ -451,6 +477,9 @@ void IgnitionROS2ControlPlugin::Configure( resource_manager_->set_component_state(control_hardware_info[i].name, state); } + rclcpp::NodeOptions cm_node_options = controller_manager::get_cm_node_options(); + cm_node_options.context(this->dataPtr->context_); + // Create the controller manager RCLCPP_INFO(this->dataPtr->node_->get_logger(), "Loading controller_manager"); this->dataPtr->controller_manager_.reset( @@ -458,9 +487,36 @@ void IgnitionROS2ControlPlugin::Configure( std::move(resource_manager_), this->dataPtr->executor_, controllerManagerNodeName, - this->dataPtr->node_->get_namespace())); + this->dataPtr->node_->get_namespace(), + cm_node_options + ) + + ); this->dataPtr->executor_->add_node(this->dataPtr->controller_manager_); + auto parameters_client = std::make_shared( + this->dataPtr->controller_manager_, controllerManagerNodeName + ); + + for (const std::string& parameter_file_name : parameter_file_names) + { + RCLCPP_INFO( + this->dataPtr->controller_manager_->get_logger(), + "Trying to load file: " + ); + + RCLCPP_INFO( + this->dataPtr->controller_manager_->get_logger(), + parameter_file_name.data() + ); + auto parameter_map = rclcpp::parameter_map_from_yaml_file(parameter_file_name); + + + // parameters_client->load_parameters(parameter_map); + // parameters_client->load_parameters(parameter_file_name).wait(); + } + + if (!this->dataPtr->controller_manager_->has_parameter("update_rate")) { RCLCPP_ERROR_STREAM( this->dataPtr->node_->get_logger(), From d0527cbc8f96e06a61809cb573330332a91d7902 Mon Sep 17 00:00:00 2001 From: brian Date: Fri, 19 Jul 2024 17:15:29 -0700 Subject: [PATCH 2/2] approach using node options. This requires params_file parameter to be specified in the controller yaml, even if the same file --- .../src/ign_ros2_control_plugin.cpp | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index 7d0e2c02..79fa755e 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -345,13 +345,13 @@ void IgnitionROS2ControlPlugin::Configure( } std::vector argv; - for (const auto & arg : arguments) { - RCLCPP_INFO( - logger, - arg.data() - ); - argv.push_back(reinterpret_cast(arg.data())); - } + // for (const auto & arg : arguments) { + // RCLCPP_INFO( + // logger, + // arg.data() + // ); + // argv.push_back(reinterpret_cast(arg.data())); + // } // Create a default context, if not already if (!rclcpp::ok()) { @@ -365,8 +365,10 @@ void IgnitionROS2ControlPlugin::Configure( msg.data() ); - this->dataPtr->context_ = std::make_shared(); - this->dataPtr->context_->init(static_cast(argv.size()), argv.data()); + // this->dataPtr->context_ = std::make_shared(); + // this->dataPtr->context_->init(static_cast(argv.size()), argv.data()); + + this->dataPtr->context_ = rclcpp::contexts::get_global_default_context(); std::string node_name = "gz_ros2_control"; @@ -375,6 +377,7 @@ void IgnitionROS2ControlPlugin::Configure( rclcpp::NodeOptions node_options; node_options.context(this->dataPtr->context_); + node_options.arguments(arguments); this->dataPtr->node_ = rclcpp::Node::make_shared(node_name, ns, node_options); this->dataPtr->executor_ = std::make_shared(executor_options); @@ -479,6 +482,7 @@ void IgnitionROS2ControlPlugin::Configure( rclcpp::NodeOptions cm_node_options = controller_manager::get_cm_node_options(); cm_node_options.context(this->dataPtr->context_); + cm_node_options.arguments(arguments); // Create the controller manager RCLCPP_INFO(this->dataPtr->node_->get_logger(), "Loading controller_manager");