Add namespaces (#81)

* added namespaces to ros2_control Signed-off-by: Jakub Delicat <[email protected]> * removed action group Signed-off-by: Jakub Delicat <[email protected]> * Added namespace to robot description Signed-off-by: Jakub Delicat <[email protected]> * added tests for namespaces in rosbot_controller Signed-off-by: Jakub Delicat <[email protected]> * update worldlist Signed-off-by: Jakub Delicat <[email protected]> * fixed ros2_control namespaces | added namespace to ekf Signed-off-by: Jakub Delicat <[email protected]> * added mecanum Signed-off-by: Jakub Delicat <[email protected]> * added ros2_controllers to .gitignore Signed-off-by: Jakub Delicat <[email protected]> * ignored ros2_controllers during Industrial CI Signed-off-by: Jakub Delicat <[email protected]> * fixed tests Signed-off-by: Jakub Delicat <[email protected]> * added tf_prefix to imu_sensor_broadcaster Signed-off-by: Jakub Delicat <[email protected]> * copied only diff_drive and imu_broadcaster Signed-off-by: Jakub Delicat <[email protected]> * changed comments | changed timeout for controller test Signed-off-by: Jakub Delicat <[email protected]> * param changed to parameter Signed-off-by: Jakub Delicat <[email protected]> * Multi robot diff drive example Signed-off-by: Jakub Delicat <[email protected]> * imu also works Signed-off-by: Jakub Delicat <[email protected]> * Added use_multirobot_system | found imu error Signed-off-by: Jakub Delicat <[email protected]> * changed all topics to ns Signed-off-by: Jakub Delicat <[email protected]> * removed tf_prefix from xacro tests Signed-off-by: Jakub Delicat <[email protected]> * removed comments | add use_multirobot_system arg Signed-off-by: Jakub Delicat <[email protected]> * moved multi robot system controllers config to rosbot_controller Signed-off-by: Jakub Delicat <[email protected]> * fixed rosbot_gazebo test Signed-off-by: Jakub Delicat <[email protected]> * separated namespaced tests | added multirobot gazebo example with broken imu Signed-off-by: Jakub Delicat <[email protected]> * add multicontroller test Signed-off-by: Jakub Delicat <[email protected]> * add multibringup test Signed-off-by: Jakub Delicat <[email protected]> * add multibringup test Signed-off-by: Jakub Delicat <[email protected]> * Added remove rosbot_macro Signed-off-by: Jakub Delicat <[email protected]> * fixed miss spelling Signed-off-by: Jakub Delicat <[email protected]> * The change from robot_hardware_interface seems to be unnecessary Signed-off-by: Jakub Delicat <[email protected]> * moved multirobot tests to arrays Signed-off-by: Jakub Delicat <[email protected]> * move rosbot_hardware_interfaces to main Signed-off-by: Jakub Delicat <[email protected]> * Fixed len Signed-off-by: Jakub Delicat <[email protected]> * Added updated controllers info to readme Signed-off-by: Jakub Delicat <[email protected]> * Update README.md Co-authored-by: rafal-gorecki <[email protected]> * Update README.md --------- Signed-off-by: Jakub Delicat <[email protected]> Co-authored-by: rafal-gorecki <[email protected]>
husarion · Dec 1, 2023 · 5762b53 · 5762b53
1 parent 3f9cfcc
commit 5762b53
Show file tree

Hide file tree

Showing 33 changed files with 1,495 additions and 221 deletions.
diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml
@@ -39,6 +39,9 @@ jobs:
             - name: Clone installation requirements
               shell: bash
               run: python3 -m pip install -U vcstool && vcs import . < ./rosbot/rosbot_hardware.repos && vcs import . < ./rosbot/rosbot_simulation.repos
+            - name: Copy only diff_drive_controller and imu_sensor_broadcaster, waits for features from ros2-control
+              shell: bash
+              run: cp -r ros2_controllers/diff_drive_controller . && cp -r ros2_controllers/imu_sensor_broadcaster . && rm -rf ros2_controllers
             - uses: ros-industrial/industrial_ci@master
               env:
                   ROS_DISTRO: ${{matrix.ROS_DISTRO}}
diff --git a/.gitignore b/.gitignore
@@ -12,6 +12,8 @@ ros_components_description/
 rosbot_controllers/
 husarion/husarion_office_gz
 gazebosim/gz_ros2_control
+diff_drive_controller
+imu_sensor_broadcaster
 
 # pyspelling
 *.dic
diff --git a/.wordlist.txt b/.wordlist.txt
@@ -120,3 +120,8 @@ Palacios
 env
 pids
 pgrep
+namespace
+TODO
+delipl
+microros
+namespaces
diff --git a/README.md b/README.md
@@ -54,6 +54,9 @@ source /opt/ros/$ROS_DISTRO/setup.bash
 
 vcs import src < src/rosbot/rosbot_hardware.repos
 
+# Build only diff_drive_controller and imu_sensor_broadcaster from ros2_controllers
+cp -r src/ros2_controllers/diff_drive_controller src && cp -r src/ros2_controllers/imu_sensor_broadcaster src && rm -rf src/ros2_controllers
+
 rm -r src/rosbot_gazebo
 
 rosdep init
@@ -94,6 +97,9 @@ source /opt/ros/$ROS_DISTRO/setup.bash
 vcs import src < src/rosbot/rosbot_hardware.repos
 vcs import src < src/rosbot/rosbot_simulation.repos
 
+# Build only diff_drive_controller and imu_sensor_broadcaster from ros2_controllers
+cp -r src/ros2_controllers/diff_drive_controller src && cp -r src/ros2_controllers/imu_sensor_broadcaster src && rm -rf src/ros2_controllers
+
 rosdep init
 rosdep update --rosdistro $ROS_DISTRO
 rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y

diff --git a/rosbot/rosbot_hardware.repos b/rosbot/rosbot_hardware.repos
@@ -11,3 +11,10 @@ repositories:
     type: git
     url: https://github.com/husarion/rosbot_controllers
     version: main
+  # Waiting for backporting to the humble version
+  # https://github.com/ros-controls/ros2_controllers/pull/726 and
+  # https://github.com/ros-controls/ros2_controllers/pull/831
+  ros2_controllers:
+    type: git
+    url: https://github.com/delihus/ros2_controllers
+    version: humble-backport
diff --git a/rosbot/rosbot_simulation.repos b/rosbot/rosbot_simulation.repos
@@ -2,9 +2,7 @@ repositories:
   gazebosim/gz_ros2_control:
     type: git
     url: https://github.com/ros-controls/gz_ros2_control.git
-    # on branch humble hardware isn't activated
-    # recently on master API breaking change was introduced, it is necessary to use commit before this change
-    version: b296ff2f5c3758b637a70bd496fe6ed875ab03ce
+    version: humble
 
   husarion/husarion_office_gz:
     type: git

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
@@ -17,7 +17,7 @@ ekf_filter_node:
         publish_tf: true
         publish_acceleration: false
 
-        odom0: /rosbot_base_controller/odom
+        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 +29,7 @@ ekf_filter_node:
         odom0_differential: false
         odom0_relative: true
 
-        imu0: /imu_broadcaster/imu
+        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
@@ -21,6 +21,13 @@
 
 
 def generate_launch_description():
+    namespace = LaunchConfiguration("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",
@@ -55,6 +62,13 @@ def generate_launch_description():
         ),
     )
 
+    use_multirobot_system = LaunchConfiguration("use_multirobot_system")
+    declare_use_multirobot_system_arg = DeclareLaunchArgument(
+        "use_multirobot_system",
+        default_value="false",
+        description="Enable correct Ignition Gazebo configuration in URDF",
+    )
+
     controller_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
@@ -70,6 +84,8 @@ def generate_launch_description():
             "mecanum": mecanum,
             "use_gpu": use_gpu,
             "simulation_engine": simulation_engine,
+            "namespace": namespace,
+            "use_multirobot_system": use_multirobot_system,
         }.items(),
     )
 
@@ -81,13 +97,20 @@ def generate_launch_description():
         name="ekf_filter_node",
         output="screen",
         parameters=[ekf_config],
+        remappings=[
+            ("/tf", "tf"),
+            ("/tf_static", "tf_static"),
+        ],
+        namespace=namespace,
     )
 
     actions = [
+        declare_namespace_arg,
         declare_mecanum_arg,
         declare_use_sim_arg,
         declare_use_gpu_arg,
         declare_simulation_engine_arg,
+        declare_use_multirobot_system_arg,
         SetParameter(name="use_sim_time", value=use_sim),
         controller_launch,
         robot_localization_node,

diff --git a/rosbot_bringup/package.xml b/rosbot_bringup/package.xml
@@ -25,6 +25,7 @@
 
   <test_depend>robot_localization</test_depend>
   <test_depend>rosbot_controller</test_depend>
+  <test_depend>nav_msgs</test_depend>
 
   <export>
     <build_type>ament_python</build_type>

diff --git a/rosbot_bringup/test/test_diff_drive_ekf.py b/rosbot_bringup/test/test_diff_drive_ekf.py
@@ -58,10 +58,10 @@ def test_bringup_startup_success():
         node.start_publishing_fake_hardware()
 
         node.start_node_thread()
-        msgs_received_flag = node.odom_tf_event.wait(timeout=10.0)
+        msgs_received_flag = node.odom_msg_event.wait(timeout=10.0)
         assert (
             msgs_received_flag
-        ), "Expected odom to base_link tf but it was not received. Check robot_localization!"
+        ), "Expected odometry/filtered message but it was not received. Check robot_localization!"
 
     finally:
         rclpy.shutdown()
diff --git a/rosbot_bringup/test/test_mecanum_ekf.py b/rosbot_bringup/test/test_mecanum_ekf.py
@@ -58,10 +58,10 @@ def test_bringup_startup_success():
         node.start_publishing_fake_hardware()
 
         node.start_node_thread()
-        msgs_received_flag = node.odom_tf_event.wait(timeout=10.0)
+        msgs_received_flag = node.odom_msg_event.wait(timeout=10.0)
         assert (
             msgs_received_flag
-        ), "Expected odom to base_link tf but it was not received. Check robot_localization!"
+        ), "Expected odometry/filtered message but it was not received. Check robot_localization!"
 
     finally:
         rclpy.shutdown()
diff --git a/rosbot_bringup/test/test_multirobot_ekf.py b/rosbot_bringup/test/test_multirobot_ekf.py
@@ -0,0 +1,79 @@
+# Copyright 2021 Open Source Robotics Foundation, Inc.
+# Copyright 2023 Intel Corporation. All Rights Reserved.
+# Copyright 2023 Husarion
+#
+# 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 launch_pytest
+import pytest
+import rclpy
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, TimerAction
+from launch.substitutions import PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from test_utils import BringupTestNode
+
+robot_names = ["rosbot1", "rosbot2", "rosbot3", "rosbot4"]
+
+
+@launch_pytest.fixture
+def generate_test_description():
+    rosbot_bringup = get_package_share_directory("rosbot_bringup")
+    actions = []
+    for i in range(len(robot_names)):
+        bringup_launch = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        rosbot_bringup,
+                        "launch",
+                        "bringup.launch.py",
+                    ]
+                )
+            ),
+            launch_arguments={
+                "use_sim": "False",
+                "mecanum": "False",
+                "use_gpu": "False",
+                "namespace": robot_names[i],
+            }.items(),
+        )
+        if i > 0:
+            delayed_bringup_launch = TimerAction(period=i * 10.0, actions=[bringup_launch])
+            actions.append(delayed_bringup_launch)
+        else:
+            actions.append(bringup_launch)
+
+    return LaunchDescription(actions)
+
+
+@pytest.mark.launch(fixture=generate_test_description)
+def test_multirobot_bringup_startup_success():
+    for robot_name in robot_names:
+        rclpy.init()
+        try:
+            node = BringupTestNode("test_bringup", namespace=robot_name)
+            node.create_test_subscribers_and_publishers()
+            node.start_publishing_fake_hardware()
+
+            node.start_node_thread()
+            msgs_received_flag = node.odom_msg_event.wait(timeout=20.0)
+            assert msgs_received_flag, (
+                f"Expected {robot_name}/odometry/filtered message but it was not received. "
+                "Check robot_localization!"
+            )
+
+        finally:
+            rclpy.shutdown()
diff --git a/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py b/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py
@@ -0,0 +1,68 @@
+# Copyright 2021 Open Source Robotics Foundation, Inc.
+# Copyright 2023 Intel Corporation. All Rights Reserved.
+# Copyright 2023 Husarion
+#
+# 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 launch_pytest
+import pytest
+import rclpy
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from test_utils import BringupTestNode
+
+
+@launch_pytest.fixture
+def generate_test_description():
+    rosbot_bringup = get_package_share_directory("rosbot_bringup")
+    bringup_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    rosbot_bringup,
+                    "launch",
+                    "bringup.launch.py",
+                ]
+            )
+        ),
+        launch_arguments={
+            "use_sim": "False",
+            "mecanum": "False",
+            "use_gpu": "False",
+            "namespace": "rosbot2r",
+        }.items(),
+    )
+
+    return LaunchDescription([bringup_launch])
+
+
+@pytest.mark.launch(fixture=generate_test_description)
+def test_namespaced_bringup_startup_success():
+    rclpy.init()
+    try:
+        node = BringupTestNode("test_bringup", namespace="rosbot2r")
+        node.create_test_subscribers_and_publishers()
+        node.start_publishing_fake_hardware()
+
+        node.start_node_thread()
+        msgs_received_flag = node.odom_msg_event.wait(timeout=10.0)
+        assert (
+            msgs_received_flag
+        ), "Expected odometry/filtered message but it was not received. Check robot_localization!"
+
+    finally:
+        rclpy.shutdown()