From 5838ce890975e3a058cdc9ab699b27941374c3a2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Pablo=20I=C3=B1igo=20Blasco?= <pablo.inigo.blasco@gmail.com>
Date: Thu, 26 Jan 2023 13:59:40 +0100
Subject: [PATCH] Fix parameters for ros2_control namespaces (#1833)

Co-authored-by: AndyZe <andyz@utexas.edu>
Co-authored-by: Henning Kayser <henningkayser@picknik.ai>
---
 .../src/controller_manager_plugin.cpp                | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp
index 0cc3b04dcf..f57cae4d1c 100644
--- a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp
+++ b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp
@@ -256,8 +256,20 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
     if (!ns_.empty())
     {
       if (!node_->has_parameter("ros_control_namespace"))
+      {
         ns_ = node_->declare_parameter<std::string>("ros_control_namespace", "/");
+      }
+      else
+      {
+        node_->get_parameter<std::string>("ros_control_namespace", ns_);
+      }
     }
+    else if (node->has_parameter("ros_control_namespace"))
+    {
+      node_->get_parameter<std::string>("ros_control_namespace", ns_);
+      RCLCPP_INFO_STREAM(LOGGER, "Namespace for controller manager was specified, namespace: " << ns_);
+    }
+
     list_controllers_service_ = node_->create_client<controller_manager_msgs::srv::ListControllers>(
         getAbsName("controller_manager/list_controllers"));
     switch_controller_service_ = node_->create_client<controller_manager_msgs::srv::SwitchController>(