fixed ros2_control namespaces | added namespace to ekf

Signed-off-by: Jakub Delicat <[email protected]>
husarion · Nov 13, 2023 · 0d82f27 · 0d82f27
1 parent 72bef34
commit 0d82f27
Show file tree

Hide file tree

Showing 6 changed files with 82 additions and 21 deletions.
diff --git a/rosbot_bringup/config/ekf.yaml b/rosbot_bringup/config/ekf.yaml
@@ -1,7 +1,7 @@
 # Ref: http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html
 
 ## ekf config file ###
-ekf_filter_node:
+/**/ekf_filter_node:
     ros__parameters:
         frequency: 25.0
         sensor_timeout: 0.05
@@ -10,14 +10,16 @@ ekf_filter_node:
         transform_time_offset: 0.0
         transform_timeout: 0.05
 
+        # All transforms are overwritten in launch file due to the namespace argument
         map_frame: map
         odom_frame: odom
         base_link_frame: base_link
         world_frame: odom
         publish_tf: true
         publish_acceleration: false
 
-        odom0: /rosbot_base_controller/odom
+        # The odometry topic name is overwritten in launch file due to the namespace argument
+        odom0: rosbot_base_controller/odom
         odom0_config: [false, false, false, # X , Y , Z
                       false, false, false, # roll , pitch ,yaw
                       true, true, false, # dX , dY , dZ
@@ -29,7 +31,8 @@ ekf_filter_node:
         odom0_differential: false
         odom0_relative: true
 
-        imu0: /imu_broadcaster/imu
+        # The IMU topic name is overwritten in launch file due to the namespace argument
+        imu0: imu_broadcaster/imu
         imu0_config: [false, false, false, # X , Y , Z
                       false, false, true, # roll , pitch ,yaw
                       false, false, false, # dX , dY , dZ

diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py
@@ -17,10 +17,20 @@
 from ament_index_python.packages import get_package_share_directory
 from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
 
 
 def generate_launch_description():
+    namespace = LaunchConfiguration("namespace")
+    namespace_tf_prefix = PythonExpression(
+        ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "_'"]
+    )
+    declare_namespace_arg = DeclareLaunchArgument(
+        "namespace",
+        default_value="",
+        description="Namespace for all topics and tfs",
+    )
+
     use_sim = LaunchConfiguration("use_sim")
     declare_use_sim_arg = DeclareLaunchArgument(
         "use_sim",
@@ -70,6 +80,7 @@ def generate_launch_description():
             "mecanum": mecanum,
             "use_gpu": use_gpu,
             "simulation_engine": simulation_engine,
+            "namespace": namespace,
         }.items(),
     )
 
@@ -80,10 +91,34 @@ def generate_launch_description():
         executable="ekf_node",
         name="ekf_filter_node",
         output="screen",
-        parameters=[ekf_config],
+        parameters=[
+            ekf_config,
+            {
+                "map_frame": LaunchConfiguration(
+                    "ekf_map_frame", default=[namespace_tf_prefix, "map"]
+                )
+            },
+            {
+                "odom_frame": LaunchConfiguration(
+                    "ekf_odom_frame", default=[namespace_tf_prefix, "odom"]
+                )
+            },
+            {
+                "base_link_frame": LaunchConfiguration(
+                    "ekf_base_link_frame", default=[namespace_tf_prefix, "base_link"]
+                )
+            },
+            {
+                "world_frame": LaunchConfiguration(
+                    "ekf_world_frame", default=[namespace_tf_prefix, "odom"]
+                )
+            },
+        ],
+        namespace=namespace,
     )
 
     actions = [
+        declare_namespace_arg,
         declare_mecanum_arg,
         declare_use_sim_arg,
         declare_use_gpu_arg,

diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml
@@ -32,13 +32,15 @@ simulation_ignition_ros_control:
 /**/imu_broadcaster:
   ros__parameters:
     sensor_name: imu
-    frame_id: imu_link
+    # All transforms are overwritten in launch file due to the namespace argument
+    # frame_id: imu_link
     static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet
     static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally
     static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally
 
 /**/rosbot_base_controller:
   ros__parameters:
+    tf_frame_prefix_enable: True
     left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"]
     right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"]
 

diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py
@@ -84,15 +84,12 @@ def generate_launch_description():
         ]
     )
 
-    controller_manager_name = LaunchConfiguration("controller_manager_name")
     namespace_for_controller_name = PythonExpression(
         ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "/'"]
     )
-
-    declare_controller_manager_name_arg = DeclareLaunchArgument(
+    controller_manager_name = LaunchConfiguration(
         "controller_manager_name",
-        default_value=[namespace_for_controller_name, controller_manager_type_name],
-        description="ros2_control controller manager name",
+        default=[namespace_for_controller_name, controller_manager_type_name],
     )
 
     # Get URDF via xacro
@@ -132,7 +129,29 @@ def generate_launch_description():
     control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[robot_description, robot_controllers],
+        parameters=[
+            robot_description,
+            robot_controllers,
+            # imu_broadcaster frame_id override
+            {"frame_id": LaunchConfiguration("imu_frame", default=[namespace, "_imu_link"])},
+            {
+                "tf_frame_prefix": LaunchConfiguration(
+                    "diff_drive_tf_frame_prefix", default=[namespace]
+                )
+            },
+            {
+                "left_wheel_names": LaunchConfiguration(
+                    "left_wheels_joints",
+                    default=["[", namespace, "_fl_wheel_joint,", namespace, "_rl_wheel_joint]"],
+                )
+            },
+            {
+                "right_wheel_names": LaunchConfiguration(
+                    "right_wheels_joints",
+                    default=["[", namespace, "_fr_wheel_joint,", namespace, "_rr_wheel_joint]"],
+                )
+            },
+        ],
         remappings=[
             ("imu_sensor_node/imu", "/_imu/data_raw"),
             ("~/motors_cmd", "/_motors_cmd"),
@@ -216,7 +235,6 @@ def generate_launch_description():
             declare_use_sim_arg,
             declare_use_gpu_arg,
             declare_simulation_engine_arg,
-            declare_controller_manager_name_arg,
             SetParameter("use_sim_time", value=use_sim),
             control_node,
             robot_state_pub_node,

diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro
@@ -60,12 +60,13 @@
           <param name="connection_timeout_ms">120000</param>
           <param name="connection_check_period_ms">500</param>
 
+          <!-- Namespaces have not been implemented in microros jet -->
           <!-- order of velocity commands to be published in motors_cmd Float32MultiArray msg -->
           <param name="velocity_command_joint_order">
-                ${tf_prefix_ext}rr_wheel_joint,
-                ${tf_prefix_ext}rl_wheel_joint,
-                ${tf_prefix_ext}fr_wheel_joint,
-                ${tf_prefix_ext}fl_wheel_joint
+                rr_wheel_joint,
+                rl_wheel_joint,
+                fr_wheel_joint,
+                fl_wheel_joint
           </param>
         </xacro:unless>
 
@@ -79,22 +80,23 @@
         </xacro:if>
       </hardware>
 
-      <joint name="${tf_prefix_ext}fl_wheel_joint">
+       <!-- Namespaces have not been implemented in microros jet -->
+      <joint name="fl_wheel_joint">
         <command_interface name="velocity" />
         <state_interface name="position" />
         <state_interface name="velocity" />
       </joint>
-      <joint name="${tf_prefix_ext}fr_wheel_joint">
+      <joint name="fr_wheel_joint">
         <command_interface name="velocity" />
         <state_interface name="position" />
         <state_interface name="velocity" />
       </joint>
-      <joint name="${tf_prefix_ext}rl_wheel_joint">
+      <joint name="rl_wheel_joint">
         <command_interface name="velocity" />
         <state_interface name="position" />
         <state_interface name="velocity" />
       </joint>
-      <joint name="${tf_prefix_ext}rr_wheel_joint">
+      <joint name="rr_wheel_joint">
         <command_interface name="velocity" />
         <state_interface name="position" />
         <state_interface name="velocity" />

diff --git a/rosbot_description/urdf/wheel.urdf.xacro b/rosbot_description/urdf/wheel.urdf.xacro
@@ -65,6 +65,7 @@
       <xacro:property name="fdir" value="1.0 -1.0 0.0" />
     </xacro:if>
 
+    <!-- Namespaces have not been implemented in microros jet -->
     <joint name="${tf_prefix_ext}${side}_wheel_joint" type="continuous">
       <parent link="${tf_prefix_ext}body_link" />
       <child link="${tf_prefix_ext}${side}_wheel_link" />