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(),