Add namespacing support

canopen_core/include/canopen_core/device_container.hpp

@@ -135,8 +135,9 @@ class DeviceContainer : public rclcpp_components::ComponentManager
    * @return false
    */
   virtual bool load_component(
-    std::string & package_name, std::string & driver_name, uint16_t node_id,
-    std::string & node_name, std::vector<rclcpp::Parameter> & params);
+    const std::string package_name, const std::string driver_name, const uint16_t node_id,
+    const std::string node_name, std::vector<rclcpp::Parameter> & params,
+    const std::string node_namespace = "");
 
   /**
    * @brief Shutdown all devices.

canopen_core/launch/canopen.launch.py

@@ -55,6 +55,11 @@ def generate_launch_description():
         default_value=TextSubstitution(text="vcan0"),
         description="CAN interface used by master and drivers.",
     )
+    namespace_arg = DeclareLaunchArgument(
+        "namespace",
+        default_value=TextSubstitution(text=""),
+        description="Namespace for the device container node.",
+    )
 
     ld = launch.LaunchDescription()
     logging = launch.actions.GroupAction(
@@ -67,7 +72,7 @@ def generate_launch_description():
     )
     lifecycle_device_container_node = launch_ros.actions.LifecycleNode(
         name="device_container_node",
-        namespace="",
+        namespace=LaunchConfiguration("namespace"),
         package="canopen_core",
         output="screen",
         executable="device_container_node",
@@ -83,6 +88,7 @@ def generate_launch_description():
     ld.add_action(master_conf_arg)
     ld.add_action(master_bin_arg)
     ld.add_action(can_interface_name_arg)
+    ld.add_action(namespace_arg)
     ld.add_action(logging)
     ld.add_action(lifecycle_device_container_node)
 

canopen_core/src/device_container.cpp

@@ -32,8 +32,9 @@ bool DeviceContainer::init_driver(uint16_t node_id)
 }
 
 bool DeviceContainer::load_component(
-  std::string & package_name, std::string & driver_name, uint16_t node_id, std::string & node_name,
-  std::vector<rclcpp::Parameter> & params)
+  const std::string package_name, const std::string driver_name, const uint16_t node_id,
+  const std::string node_name, std::vector<rclcpp::Parameter> & params,
+  const std::string node_namespace)
 {
   ComponentResource component;
   std::string resource_index("rclcpp_components");
@@ -54,7 +55,15 @@ bool DeviceContainer::load_component(
       // remap_rules.push_back("debug");
       remap_rules.push_back("-r");
       remap_rules.push_back("__node:=" + node_name);
-
+      remap_rules.push_back("-r");
+      if (node_namespace.compare("") == 0)
+      {
+        remap_rules.push_back("__ns:=" + std::string(get_namespace()));
+      }
+      else
+      {
+        remap_rules.push_back("__ns:=" + node_namespace);
+      }
       opts.arguments(remap_rules);
       opts.parameter_overrides(params);
 
@@ -210,7 +219,7 @@ bool DeviceContainer::load_master()
       auto node_id = config_->get_entry<uint16_t>(*it, "node_id");
       auto driver_name = config_->get_entry<std::string>(*it, "driver");
       auto package_name = config_->get_entry<std::string>(*it, "package");
-
+      auto node_namespace = config_->get_entry<std::string>(*it, "namespace").value_or("");
       if (!node_id.has_value() || !driver_name.has_value() || !package_name.has_value())
       {
         RCLCPP_ERROR(
@@ -227,7 +236,8 @@ bool DeviceContainer::load_master()
       params.push_back(rclcpp::Parameter("config", config_->dump_device(*it)));
 
       if (!this->load_component(
-            package_name.value(), driver_name.value(), node_id.value(), *it, params))
+            package_name.value(), driver_name.value(), node_id.value(), *it, params,
+            node_namespace))
       {
         RCLCPP_ERROR(this->get_logger(), "Error: Loading master failed.");
         return false;
@@ -266,6 +276,7 @@ bool DeviceContainer::load_drivers()
       auto node_id = config_->get_entry<uint16_t>(*it, "node_id");
       auto driver_name = config_->get_entry<std::string>(*it, "driver");
       auto package_name = config_->get_entry<std::string>(*it, "package");
+      auto node_namespace = config_->get_entry<std::string>(*it, "namespace").value_or("");
       if (!node_id.has_value() || !driver_name.has_value() || !package_name.has_value())
       {
         RCLCPP_ERROR(
@@ -301,7 +312,8 @@ bool DeviceContainer::load_drivers()
       params.push_back(rclcpp::Parameter("non_transmit_timeout", 100));
 
       if (!this->load_component(
-            package_name.value(), driver_name.value(), node_id.value(), *it, params))
+            package_name.value(), driver_name.value(), node_id.value(), *it, params,
+            node_namespace))
       {
         RCLCPP_ERROR(
           this->get_logger(),

canopen_tests/config/simple/bus.yml

@@ -13,6 +13,7 @@ proxy_device_1:
   package: "canopen_proxy_driver"
   polling: true
   period: 10
+  namespace: "/xxx"
 
 proxy_device_2:
   node_id: 3

canopen_tests/launch/proxy_setup_namespaced.launch.py

@@ -0,0 +1,91 @@
+#    Copyright 2022 Christoph Hellmann Santos
+#
+#    Licensed under the Apache License, Version 2.0 (the "License");
+#    you may not use this file except in compliance with the License.
+#    You may obtain a copy of the License at
+#
+#        http://www.apache.org/licenses/LICENSE-2.0
+#
+#    Unless required by applicable law or agreed to in writing, software
+#    distributed under the License is distributed on an "AS IS" BASIS,
+#    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#    See the License for the specific language governing permissions and
+#    limitations under the License.
+
+import os
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription
+import launch
+import launch_ros
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    slave_eds_path = os.path.join(
+        get_package_share_directory("canopen_tests"), "config", "simple", "simple.eds"
+    )
+    slave_node_1 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"),
+                "/basic_slave.launch.py",
+            ]
+        ),
+        launch_arguments={
+            "node_id": "2",
+            "node_name": "slave_node_1",
+            "slave_config": slave_eds_path,
+        }.items(),
+    )
+
+    slave_node_2 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"),
+                "/basic_slave.launch.py",
+            ]
+        ),
+        launch_arguments={
+            "node_id": "3",
+            "node_name": "slave_node_2",
+            "slave_config": slave_eds_path,
+        }.items(),
+    )
+
+    print(
+        os.path.join(
+            get_package_share_directory("canopen_tests"),
+            "config",
+            "proxy_write_sdo",
+            "master.dcf",
+        )
+    )
+
+    device_container = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                os.path.join(get_package_share_directory("canopen_core"), "launch"),
+                "/canopen.launch.py",
+            ]
+        ),
+        launch_arguments={
+            "master_config": os.path.join(
+                get_package_share_directory("canopen_tests"),
+                "config",
+                "simple",
+                "master.dcf",
+            ),
+            "master_bin": "",
+            "bus_config": os.path.join(
+                get_package_share_directory("canopen_tests"),
+                "config",
+                "simple",
+                "bus.yml",
+            ),
+            "can_interface_name": "vcan0",
+            "namespace": "/test_ns",
+        }.items(),
+    )
+
+    return LaunchDescription([slave_node_1, slave_node_2, device_container])